diff options
Diffstat (limited to 'src')
68 files changed, 5143 insertions, 2164 deletions
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index aeaf830de..4a6f30a4f 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -55,6 +55,7 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_safety.h> #include <systemlib/systemlib.h> @@ -247,13 +248,13 @@ int ardrone_interface_thread_main(int argc, char *argv[]) memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); - struct actuator_armed_s armed; - armed.armed = false; + struct actuator_safety_s safety; + safety.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); @@ -322,16 +323,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } else { /* MAIN OPERATION MODE */ - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); /* for now only spin if armed and immediately shut down * if in failsafe */ - if (armed.armed && !armed.lockdown) { + if (safety.armed && !safety.lockdown) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 9fed1149d..a11f88477 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -116,6 +116,7 @@ #include <poll.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_safety.h> #include <uORB/topics/vehicle_gps_position.h> static const float MAX_CELL_VOLTAGE = 4.3f; @@ -376,6 +377,7 @@ BlinkM::led() static int vehicle_status_sub_fd; static int vehicle_gps_position_sub_fd; + static int actuator_safety_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -386,6 +388,7 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; + static int no_data_actuator_safety = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -398,6 +401,9 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); + actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(actuator_safety_sub_fd, 1000); + vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); @@ -452,12 +458,14 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; + struct actuator_safety_s actuator_safety; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; + bool new_data_actuator_safety; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -471,6 +479,17 @@ BlinkM::led() no_data_vehicle_status = 3; } + orb_check(actuator_safety_sub_fd, &new_data_actuator_safety); + + if (new_data_actuator_safety) { + orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety); + no_data_actuator_safety = 0; + } else { + no_data_actuator_safety++; + if(no_data_vehicle_status >= 3) + no_data_vehicle_status = 3; + } + orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); if (new_data_vehicle_gps_position) { @@ -530,7 +549,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(vehicle_status_raw.flag_system_armed == false) { + if(actuator_safety.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -555,26 +574,27 @@ BlinkM::led() led_blink = LED_BLINK; /* handle 4th led - flightmode indicator */ - switch((int)vehicle_status_raw.flight_mode) { - case VEHICLE_FLIGHT_MODE_MANUAL: - led_color_4 = LED_AMBER; - break; - - case VEHICLE_FLIGHT_MODE_STAB: - led_color_4 = LED_YELLOW; - break; - - case VEHICLE_FLIGHT_MODE_HOLD: - led_color_4 = LED_BLUE; - break; - - case VEHICLE_FLIGHT_MODE_AUTO: - led_color_4 = LED_GREEN; - break; - } +#warning fix this +// switch((int)vehicle_status_raw.flight_mode) { +// case VEHICLE_FLIGHT_MODE_MANUAL: +// led_color_4 = LED_AMBER; +// break; +// +// case VEHICLE_FLIGHT_MODE_STAB: +// led_color_4 = LED_YELLOW; +// break; +// +// case VEHICLE_FLIGHT_MODE_HOLD: +// led_color_4 = LED_BLUE; +// break; +// +// case VEHICLE_FLIGHT_MODE_AUTO: +// led_color_4 = LED_GREEN; +// break; +// } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { - /* handling used sat´s */ + /* handling used sat�s */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index e50395e47..c39da98d7 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -37,7 +37,7 @@ * * Driver for the Eagle Tree Airspeed V3 connected via I2C. */ - + #include <nuttx/config.h> #include <drivers/device/i2c.h> @@ -77,13 +77,13 @@ #define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION /* I2C bus address */ -#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ +#define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ /* Register address */ -#define READ_CMD 0x07 /* Read the data */ - +#define READ_CMD 0x07 /* Read the data */ + /** - * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. + * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h. */ #define MIN_ACCURATE_DIFF_PRES_PA 12 @@ -105,38 +105,38 @@ class ETSAirspeed : public device::I2C public: ETSAirspeed(int bus, int address = I2C_ADDRESS); virtual ~ETSAirspeed(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + /** * Diagnostics - print some basic information about the driver. */ - void print_info(); - + void print_info(); + protected: - virtual int probe(); + virtual int probe(); private: - work_s _work; - unsigned _num_reports; - volatile unsigned _next_report; - volatile unsigned _oldest_report; - differential_pressure_s *_reports; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; - int _diff_pres_offset; - - orb_advert_t _airspeed_pub; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - - + work_s _work; + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + differential_pressure_s *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _diff_pres_offset; + + orb_advert_t _airspeed_pub; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** * Test whether the device supported by the driver is present at a * specific address. @@ -144,28 +144,28 @@ private: * @param address The I2C bus address to probe. * @return True if the device is present. */ - int probe_address(uint8_t address); - + int probe_address(uint8_t address); + /** * Initialise the automatic measurement state machine and start it. * * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start(); - + void start(); + /** * Stop the automatic measurement state machine. */ - void stop(); - + void stop(); + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. */ - void cycle(); - int measure(); - int collect(); + void cycle(); + int measure(); + int collect(); /** * Static trampoline from the workq context; because we don't have a @@ -173,9 +173,9 @@ private: * * @param arg Instance pointer for the driver that is polling. */ - static void cycle_trampoline(void *arg); - - + static void cycle_trampoline(void *arg); + + }; /* helper macro for handling report buffer indices */ @@ -203,7 +203,7 @@ ETSAirspeed::ETSAirspeed(int bus, int address) : { // enable debug() calls _debug_enabled = true; - + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -230,6 +230,7 @@ ETSAirspeed::init() /* allocate basic report buffers */ _num_reports = 2; _reports = new struct differential_pressure_s[_num_reports]; + for (unsigned i = 0; i < _num_reports; i++) _reports[i].max_differential_pressure_pa = 0; @@ -351,11 +352,11 @@ ETSAirspeed::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCGQUEUEDEPTH: return _num_reports - 1; - + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; - + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -432,14 +433,14 @@ ETSAirspeed::measure() uint8_t cmd = READ_CMD; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); return ret; } + ret = OK; - + return ret; } @@ -447,30 +448,31 @@ int ETSAirspeed::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[2] = {0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 2); - + if (ret < 0) { log("error reading from sensor: %d", ret); return ret; } - + uint16_t diff_pres_pa = val[1] << 8 | val[0]; param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); - - if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { + + if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { diff_pres_pa = 0; + } else { - diff_pres_pa -= _diff_pres_offset; + diff_pres_pa -= _diff_pres_offset; } - // XXX we may want to smooth out the readings to remove noise. + // XXX we may want to smooth out the readings to remove noise. _reports[_next_report].timestamp = hrt_absolute_time(); _reports[_next_report].differential_pressure_pa = diff_pres_pa; @@ -498,7 +500,7 @@ ETSAirspeed::collect() ret = OK; perf_end(_sample_perf); - + return ret; } @@ -511,17 +513,19 @@ ETSAirspeed::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&ETSAirspeed::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, true, true, - SUBSYSTEM_TYPE_DIFFPRESSURE}; + SUBSYSTEM_TYPE_DIFFPRESSURE + }; static orb_advert_t pub = -1; if (pub > 0) { orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -653,8 +657,7 @@ start(int i2c_bus) fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -668,15 +671,14 @@ fail: void stop() { - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } - else - { + + } else { errx(1, "driver not running"); } + exit(0); } @@ -773,10 +775,10 @@ info() } // namespace -static void -ets_airspeed_usage() +static void +ets_airspeed_usage() { - fprintf(stderr, "usage: ets_airspeed [options] command\n"); + fprintf(stderr, "usage: ets_airspeed command [options]\n"); fprintf(stderr, "options:\n"); fprintf(stderr, "\t-b --bus i2cbus (%d)\n", PX4_I2C_BUS_DEFAULT); fprintf(stderr, "command:\n"); @@ -789,6 +791,7 @@ ets_airspeed_main(int argc, char *argv[]) int i2c_bus = PX4_I2C_BUS_DEFAULT; int i; + for (i = 1; i < argc; i++) { if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { if (argc > i + 1) { @@ -802,12 +805,12 @@ ets_airspeed_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) ets_airspeed::start(i2c_bus); - - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - ets_airspeed::stop(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + ets_airspeed::stop(); /* * Test the driver/device. diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index d9aa772d4..e851d8a52 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,6 +75,7 @@ #include <systemlib/mixer/mixer.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_safety.h> #include <uORB/topics/actuator_outputs.h> #include <systemlib/err.h> @@ -108,7 +109,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_safety; orb_advert_t _t_outputs; unsigned _num_outputs; bool _primary_pwm_device; @@ -161,7 +162,7 @@ HIL::HIL() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_safety(-1), _t_outputs(0), _num_outputs(0), _primary_pwm_device(false), @@ -321,8 +322,8 @@ HIL::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -334,7 +335,7 @@ HIL::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_safety; fds[1].events = POLLIN; unsigned num_outputs; @@ -426,15 +427,15 @@ HIL::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_safety, &aa); } } ::close(_t_actuators); - ::close(_t_armed); + ::close(_t_safety); /* make sure servos are off */ // up_pwm_servo_deinit(); diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c index 369070f8c..d2634ef41 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott_telemetry/messages.c @@ -44,6 +44,7 @@ #include <string.h> #include <systemlib/geo/geo.h> #include <unistd.h> +#include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/home_position.h> #include <uORB/topics/sensor_combined.h> @@ -56,6 +57,7 @@ static int battery_sub = -1; static int gps_sub = -1; static int home_sub = -1; static int sensor_sub = -1; +static int airspeed_sub = -1; static bool home_position_set = false; static double home_lat = 0.0d; @@ -68,6 +70,7 @@ messages_init(void) gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); home_sub = orb_subscribe(ORB_ID(home_position)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + airspeed_sub = orb_subscribe(ORB_ID(airspeed)); } void @@ -100,6 +103,16 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; + /* get a local copy of the airspeed data */ + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); + orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); + + uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s); + msg.speed_L = (uint8_t)speed & 0xff; + msg.speed_H = (uint8_t)(speed >> 8) & 0xff; + + msg.stop = STOP_BYTE; memcpy(buffer, &msg, *size); } diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index c67276f8a..093b53d42 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_safety.h> #include <systemlib/err.h> @@ -132,7 +133,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_safety; unsigned int _motor; int _px4mode; int _frametype; @@ -240,7 +241,7 @@ MK::MK(int bus) : _update_rate(50), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -496,8 +497,8 @@ MK::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -519,7 +520,7 @@ MK::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; log("starting"); @@ -625,13 +626,13 @@ MK::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s as; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); /* update PWM servo armed status if armed and not locked down */ - mk_servo_arm(aa.armed && !aa.lockdown); + mk_servo_arm(as.armed && !as.lockdown); } @@ -639,7 +640,7 @@ MK::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_safety); /* make sure servos are off */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bf72892eb..db4c87ddc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,6 +69,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_safety.h> #include <systemlib/err.h> #include <systemlib/ppm_decode.h> @@ -104,7 +105,7 @@ private: unsigned _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_safety; orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; unsigned _num_outputs; @@ -174,7 +175,7 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -376,8 +377,8 @@ PX4FMU::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -396,7 +397,7 @@ PX4FMU::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; @@ -499,13 +500,13 @@ PX4FMU::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s as; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); /* update PWM servo armed status if armed and not locked down */ - bool set_armed = aa.armed && !aa.lockdown; + bool set_armed = as.armed && !as.lockdown; if (set_armed != _armed) { _armed = set_armed; up_pwm_servo_arm(set_armed); @@ -535,7 +536,7 @@ PX4FMU::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_safety); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -1021,12 +1022,12 @@ fake(int argc, char *argv[]) if (handle < 0) errx(1, "advertise failed"); - actuator_armed_s aa; + actuator_safety_s as; - aa.armed = true; - aa.lockdown = false; + as.armed = true; + as.lockdown = false; - handle = orb_advertise(ORB_ID(actuator_armed), &aa); + handle = orb_advertise(ORB_ID(actuator_safety), &as); if (handle < 0) errx(1, "advertise failed 2"); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 925041e0c..bce193bca 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,6 +75,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_safety.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/rc_channels.h> @@ -126,6 +127,16 @@ public: int set_failsafe_values(const uint16_t *vals, unsigned len); /** + * Set the minimum PWM signals when armed + */ + int set_min_values(const uint16_t *vals, unsigned len); + + /** + * Set the maximum PWM signal when armed + */ + int set_max_values(const uint16_t *vals, unsigned len); + + /** * Print the current status of IO */ void print_status(); @@ -153,7 +164,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic - int _t_armed; ///< system armed control topic + int _t_actuator_safety; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic @@ -329,7 +340,7 @@ PX4IO::PX4IO() : _status(0), _alarms(0), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_vstatus(-1), _t_param(-1), _to_input_rc(0), @@ -454,7 +465,8 @@ PX4IO::init() ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE CUSTOM" : "")); /* @@ -472,20 +484,20 @@ PX4IO::init() * remains untouched (so manual override is still available). */ - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); /* fill with initial values, clear updated flag */ - vehicle_status_s status; + struct actuator_safety_s safety; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; - /* keep checking for an update, ensure we got a recent state, + /* keep checking for an update, ensure we got a arming information, not something that was published a long time ago. */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { /* got data, copy and exit loop */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); break; } @@ -511,10 +523,10 @@ PX4IO::init() cmd.param6 = 0; cmd.param7 = 0; cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; - cmd.target_system = status.system_id; - cmd.target_component = status.component_id; - cmd.source_system = status.system_id; - cmd.source_component = status.component_id; + // cmd.target_system = status.system_id; + // cmd.target_component = status.component_id; + // cmd.source_system = status.system_id; + // cmd.source_component = status.component_id; /* ask to confirm command */ cmd.confirmation = 1; @@ -523,10 +535,10 @@ PX4IO::init() /* spin here until IO's state has propagated into the system */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); } /* wait 50 ms */ @@ -539,13 +551,13 @@ PX4IO::init() } /* re-send if necessary */ - if (!status.flag_system_armed) { + if (!safety.armed) { orb_publish(ORB_ID(vehicle_command), pub, &cmd); log("re-sending arm cmd"); } /* keep waiting for state change for 2 s */ - } while (!status.flag_system_armed); + } while (!safety.armed); /* regular boot, no in-air restart, init IO */ } else { @@ -609,8 +621,8 @@ PX4IO::task_main() ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ @@ -622,7 +634,7 @@ PX4IO::task_main() pollfd fds[4]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; fds[2].fd = _t_vstatus; fds[2].events = POLLIN; @@ -756,24 +768,51 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) } int +PX4IO::set_min_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len); +} + +int +PX4IO::set_max_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len); +} + + + +int PX4IO::io_set_arming_state() { - actuator_armed_s armed; ///< system armed state + actuator_safety_s safety; ///< system armed state vehicle_status_s vstatus; ///< overall system state - orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); uint16_t set = 0; uint16_t clear = 0; - if (armed.armed && !armed.lockdown) { + if (safety.armed && !safety.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - - if (armed.ready_to_arm) { + if (safety.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -1248,6 +1287,7 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t uint16_t value; ret = io_reg_get(page, offset, &value, 1); + if (ret) return ret; value &= ~clearbits; @@ -1863,6 +1903,76 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "min")) { + + if (argc < 3) { + errx(1, "min command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 900. */ + uint16_t min[8]; + + for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++) + { + /* set channel to commanline argument or to 900 for non-provided channels */ + if (argc > i + 2) { + min[i] = atoi(argv[i+2]); + if (min[i] < 900 || min[i] > 1200) { + errx(1, "value out of range of 900 < value < 1200. Aborting."); + } + } else { + /* a zero value will the default */ + min[i] = 0; + } + } + + int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0])); + + if (ret != OK) + errx(ret, "failed setting min values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + + if (!strcmp(argv[1], "max")) { + + if (argc < 3) { + errx(1, "max command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 2100. */ + uint16_t max[8]; + + for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++) + { + /* set channel to commanline argument or to 2100 for non-provided channels */ + if (argc > i + 2) { + max[i] = atoi(argv[i+2]); + if (max[i] < 1800 || max[i] > 2100) { + errx(1, "value out of range of 1800 < value < 2100. Aborting."); + } + } else { + /* a zero value will the default */ + max[i] = 0; + } + } + + int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0])); + + if (ret != OK) + errx(ret, "failed setting max values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 89fbef020..6f52c60bb 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -380,9 +380,10 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* control */ - /* if in auto mode, fly global position setpoint */ - if (vstatus.state_machine == SYSTEM_STATE_AUTO || - vstatus.state_machine == SYSTEM_STATE_STABILIZED) { +#warning fix this +#if 0 + if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ || + vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) { /* simple heading control */ control_heading(&global_pos, &global_sp, &att, &att_sp); @@ -400,6 +401,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) /* set flaps to zero */ actuators.control[4] = 0.0f; + } else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) { /* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */ } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { @@ -465,6 +467,7 @@ int fixedwing_control_thread_main(int argc, char *argv[]) } } } +#endif /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c new file mode 100644 index 000000000..5246ea62b --- /dev/null +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -0,0 +1,591 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file flow_position_control.c + * + * Optical flow position controller + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <termios.h> +#include <time.h> +#include <math.h> +#include <sys/prctl.h> +#include <drivers/drv_hrt.h> +#include <uORB/uORB.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h> +#include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/filtered_bottom_flow.h> +#include <systemlib/systemlib.h> +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <poll.h> + +#include "flow_position_control_params.h" + + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +__EXPORT int flow_position_control_main(int argc, char *argv[]); + +/** + * Mainloop of position controller. + */ +static int flow_position_control_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_spawn(). + */ +int flow_position_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) + { + if (thread_running) + { + printf("flow position control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("flow_position_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 6, + 4096, + flow_position_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) + { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) + { + if (thread_running) + printf("\tflow position control app is running\n"); + else + printf("\tflow position control app not started\n"); + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +static int +flow_position_control_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + thread_running = true; + printf("[flow position control] starting\n"); + + uint32_t counter = 0; + const float time_scale = powf(10.0f,-6.0f); + + /* structures */ + struct vehicle_status_s vstatus; + struct vehicle_attitude_s att; + struct manual_control_setpoint_s manual; + struct filtered_bottom_flow_s filtered_flow; + struct vehicle_local_position_s local_pos; + + struct vehicle_bodyframe_speed_setpoint_s speed_sp; + + /* subscribe to attitude, motor setpoints and system state */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); + int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + + orb_advert_t speed_sp_pub; + bool speed_setpoint_adverted = false; + + /* parameters init*/ + struct flow_position_control_params params; + struct flow_position_control_param_handles param_handles; + parameters_init(¶m_handles); + parameters_update(¶m_handles, ¶ms); + + /* init flow sum setpoint */ + float flow_sp_sumx = 0.0f; + float flow_sp_sumy = 0.0f; + + /* init yaw setpoint */ + float yaw_sp = 0.0f; + + /* init height setpoint */ + float height_sp = params.height_min; + + /* height controller states */ + bool start_phase = true; + bool landing_initialized = false; + float landing_thrust_start = 0.0f; + + /* states */ + float integrated_h_error = 0.0f; + float last_local_pos_z = 0.0f; + bool update_flow_sp_sumx = false; + bool update_flow_sp_sumy = false; + uint64_t last_time = 0.0f; + float dt = 0.0f; // s + + + /* register the perf counter */ + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime"); + perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval"); + perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err"); + + static bool sensors_ready = false; + + while (!thread_should_exit) + { + /* wait for first attitude msg to be sure all data are available */ + if (sensors_ready) + { + /* polling */ + struct pollfd fds[2] = { + { .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator + { .fd = parameter_update_sub, .events = POLLIN } + + }; + + /* wait for a position update, check for exit condition every 500 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } + else if (ret == 0) + { + /* no return value, ignore */ +// printf("[flow position control] no filtered flow updates\n"); + } + else + { + /* parameter update available? */ + if (fds[1].revents & POLLIN) + { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + parameters_update(¶m_handles, ¶ms); + printf("[flow position control] parameters updated.\n"); + } + + /* only run controller if position/speed changed */ + if (fds[0].revents & POLLIN) + { + perf_begin(mc_loop_perf); + + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); + /* get a local copy of attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + /* get a local copy of filtered bottom flow */ + orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); + /* get a local copy of local position */ + orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); + +// XXX fix this +#if 0 + if (vstatus.state_machine == SYSTEM_STATE_AUTO) + { + float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 + float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 + float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1 + + /* calc dt */ + if(last_time == 0) + { + last_time = hrt_absolute_time(); + continue; + } + dt = ((float) (hrt_absolute_time() - last_time)) * time_scale; + last_time = hrt_absolute_time(); + + /* update flow sum setpoint */ + if (update_flow_sp_sumx) + { + flow_sp_sumx = filtered_flow.sumx; + update_flow_sp_sumx = false; + } + if (update_flow_sp_sumy) + { + flow_sp_sumy = filtered_flow.sumy; + update_flow_sp_sumy = false; + } + + /* calc new bodyframe speed setpoints */ + float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d; + float speed_body_y = (flow_sp_sumy - filtered_flow.sumy) * params.pos_p - filtered_flow.vy * params.pos_d; + float speed_limit_height_factor = height_sp; // the settings are for 1 meter + + /* overwrite with rc input if there is any */ + if(isfinite(manual_pitch) && isfinite(manual_roll)) + { + if(fabsf(manual_pitch) > params.manual_threshold) + { + speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor; + update_flow_sp_sumx = true; + } + + if(fabsf(manual_roll) > params.manual_threshold) + { + speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor; + update_flow_sp_sumy = true; + } + } + + /* limit speed setpoints */ + if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) && + (speed_body_x >= -params.limit_speed_x * speed_limit_height_factor)) + { + speed_sp.vx = speed_body_x; + } + else + { + if(speed_body_x > params.limit_speed_x * speed_limit_height_factor) + speed_sp.vx = params.limit_speed_x * speed_limit_height_factor; + if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor) + speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor; + } + + if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) && + (speed_body_y >= -params.limit_speed_y * speed_limit_height_factor)) + { + speed_sp.vy = speed_body_y; + } + else + { + if(speed_body_y > params.limit_speed_y * speed_limit_height_factor) + speed_sp.vy = params.limit_speed_y * speed_limit_height_factor; + if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor) + speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor; + } + + /* manual yaw change */ + if(isfinite(manual_yaw) && isfinite(manual.throttle)) + { + if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f) + { + yaw_sp += manual_yaw * params.limit_yaw_step; + + /* modulo for rotation -pi +pi */ + if(yaw_sp < -M_PI_F) + yaw_sp = yaw_sp + M_TWOPI_F; + else if(yaw_sp > M_PI_F) + yaw_sp = yaw_sp - M_TWOPI_F; + } + } + + /* forward yaw setpoint */ + speed_sp.yaw_sp = yaw_sp; + + + /* manual height control + * 0-20%: thrust linear down + * 20%-40%: down + * 40%-60%: stabilize altitude + * 60-100%: up + */ + float thrust_control = 0.0f; + + if (isfinite(manual.throttle)) + { + if (start_phase) + { + /* control start thrust with stick input */ + if (manual.throttle < 0.4f) + { + /* first 40% for up to feedforward */ + thrust_control = manual.throttle / 0.4f * params.thrust_feedforward; + } + else + { + /* second 60% for up to feedforward + 10% */ + thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward; + } + + /* exit start phase if setpoint is reached */ + if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower) + { + start_phase = false; + /* switch to stabilize */ + thrust_control = params.thrust_feedforward; + } + } + else + { + if (manual.throttle < 0.2f) + { + /* landing initialization */ + if (!landing_initialized) + { + /* consider last thrust control to avoid steps */ + landing_thrust_start = speed_sp.thrust_sp; + landing_initialized = true; + } + + /* set current height as setpoint to avoid steps */ + if (-local_pos.z > params.height_min) + height_sp = -local_pos.z; + else + height_sp = params.height_min; + + /* lower 20% stick range controls thrust down */ + thrust_control = manual.throttle / 0.2f * landing_thrust_start; + + /* assume ground position here */ + if (thrust_control < 0.1f) + { + /* reset integral if on ground */ + integrated_h_error = 0.0f; + /* switch to start phase */ + start_phase = true; + /* reset height setpoint */ + height_sp = params.height_min; + } + } + else + { + /* stabilized mode */ + landing_initialized = false; + + /* calc new thrust with PID */ + float height_error = (local_pos.z - (-height_sp)); + + /* update height setpoint if needed*/ + if (manual.throttle < 0.4f) + { + /* down */ + if (height_sp > params.height_min + params.height_rate && + fabsf(height_error) < params.limit_height_error) + height_sp -= params.height_rate * dt; + } + + if (manual.throttle > 0.6f) + { + /* up */ + if (height_sp < params.height_max && + fabsf(height_error) < params.limit_height_error) + height_sp += params.height_rate * dt; + } + + /* instead of speed limitation, limit height error (downwards) */ + if(height_error > params.limit_height_error) + height_error = params.limit_height_error; + else if(height_error < -params.limit_height_error) + height_error = -params.limit_height_error; + + integrated_h_error = integrated_h_error + height_error; + float integrated_thrust_addition = integrated_h_error * params.height_i; + + if(integrated_thrust_addition > params.limit_thrust_int) + integrated_thrust_addition = params.limit_thrust_int; + if(integrated_thrust_addition < -params.limit_thrust_int) + integrated_thrust_addition = -params.limit_thrust_int; + + float height_speed = last_local_pos_z - local_pos.z; + float thrust_diff = height_error * params.height_p - height_speed * params.height_d; + + thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition; + + /* add attitude component + * F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM + */ +// // TODO problem with attitude +// if (att.R_valid && att.R[2][2] > 0) +// thrust_control = thrust_control / att.R[2][2]; + + /* set thrust lower limit */ + if(thrust_control < params.limit_thrust_lower) + thrust_control = params.limit_thrust_lower; + } + } + + /* set thrust upper limit */ + if(thrust_control > params.limit_thrust_upper) + thrust_control = params.limit_thrust_upper; + } + /* store actual height for speed estimation */ + last_local_pos_z = local_pos.z; + + speed_sp.thrust_sp = thrust_control; + speed_sp.timestamp = hrt_absolute_time(); + + /* publish new speed setpoint */ + if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp)) + { + + if(speed_setpoint_adverted) + { + orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp); + } + else + { + speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp); + speed_setpoint_adverted = true; + } + } + else + { + warnx("NaN in flow position controller!"); + } + } + else + { + /* in manual or stabilized state just reset speed and flow sum setpoint */ + speed_sp.vx = 0.0f; + speed_sp.vy = 0.0f; + flow_sp_sumx = filtered_flow.sumx; + flow_sp_sumy = filtered_flow.sumy; + if(isfinite(att.yaw)) + { + yaw_sp = att.yaw; + speed_sp.yaw_sp = att.yaw; + } + if(isfinite(manual.throttle)) + speed_sp.thrust_sp = manual.throttle; + } +#endif + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); + perf_end(mc_loop_perf); + } + } + + counter++; + } + else + { + /* sensors not ready waiting for first attitude msg */ + + /* polling */ + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + /* wait for a flow msg, check for exit condition every 5 s */ + int ret = poll(fds, 1, 5000); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } + else if (ret == 0) + { + /* no return value, ignore */ + printf("[flow position control] no attitude received.\n"); + } + else + { + if (fds[0].revents & POLLIN) + { + sensors_ready = true; + printf("[flow position control] initialized.\n"); + } + } + } + } + + printf("[flow position control] ending now...\n"); + + thread_running = false; + + close(parameter_update_sub); + close(vehicle_attitude_sub); + close(vehicle_local_position_sub); + close(vehicle_status_sub); + close(manual_control_setpoint_sub); + close(speed_sp_pub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + + fflush(stdout); + return 0; +} + diff --git a/src/examples/flow_position_control/flow_position_control_params.c b/src/examples/flow_position_control/flow_position_control_params.c new file mode 100644 index 000000000..4f3598a57 --- /dev/null +++ b/src/examples/flow_position_control/flow_position_control_params.c @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file flow_position_control_params.c + */ + +#include "flow_position_control_params.h" + +/* controller parameters */ +PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f); +PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f); +PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f); +PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f); +PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f); +PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f); +PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f); +PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f); +PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight +PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f); +PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f); +PARAM_DEFINE_FLOAT(FPC_L_H_ERR, 0.1f); +PARAM_DEFINE_FLOAT(FPC_L_TH_I, 0.05f); +PARAM_DEFINE_FLOAT(FPC_L_TH_U, 0.8f); +PARAM_DEFINE_FLOAT(FPC_L_TH_L, 0.6f); +PARAM_DEFINE_FLOAT(FPC_L_YAW_STEP, 0.03f); +PARAM_DEFINE_FLOAT(FPC_MAN_THR, 0.1f); + + +int parameters_init(struct flow_position_control_param_handles *h) +{ + /* PID parameters */ + h->pos_p = param_find("FPC_POS_P"); + h->pos_d = param_find("FPC_POS_D"); + h->height_p = param_find("FPC_H_P"); + h->height_i = param_find("FPC_H_I"); + h->height_d = param_find("FPC_H_D"); + h->height_rate = param_find("FPC_H_RATE"); + h->height_min = param_find("FPC_H_MIN"); + h->height_max = param_find("FPC_H_MAX"); + h->thrust_feedforward = param_find("FPC_T_FFWD"); + h->limit_speed_x = param_find("FPC_L_S_X"); + h->limit_speed_y = param_find("FPC_L_S_Y"); + h->limit_height_error = param_find("FPC_L_H_ERR"); + h->limit_thrust_int = param_find("FPC_L_TH_I"); + h->limit_thrust_upper = param_find("FPC_L_TH_U"); + h->limit_thrust_lower = param_find("FPC_L_TH_L"); + h->limit_yaw_step = param_find("FPC_L_YAW_STEP"); + h->manual_threshold = param_find("FPC_MAN_THR"); + h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); + h->rc_scale_roll = param_find("RC_SCALE_ROLL"); + h->rc_scale_yaw = param_find("RC_SCALE_YAW"); + + return OK; +} + +int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p) +{ + param_get(h->pos_p, &(p->pos_p)); + param_get(h->pos_d, &(p->pos_d)); + param_get(h->height_p, &(p->height_p)); + param_get(h->height_i, &(p->height_i)); + param_get(h->height_d, &(p->height_d)); + param_get(h->height_rate, &(p->height_rate)); + param_get(h->height_min, &(p->height_min)); + param_get(h->height_max, &(p->height_max)); + param_get(h->thrust_feedforward, &(p->thrust_feedforward)); + param_get(h->limit_speed_x, &(p->limit_speed_x)); + param_get(h->limit_speed_y, &(p->limit_speed_y)); + param_get(h->limit_height_error, &(p->limit_height_error)); + param_get(h->limit_thrust_int, &(p->limit_thrust_int)); + param_get(h->limit_thrust_upper, &(p->limit_thrust_upper)); + param_get(h->limit_thrust_lower, &(p->limit_thrust_lower)); + param_get(h->limit_yaw_step, &(p->limit_yaw_step)); + param_get(h->manual_threshold, &(p->manual_threshold)); + param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); + param_get(h->rc_scale_roll, &(p->rc_scale_roll)); + param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); + + return OK; +} diff --git a/src/examples/flow_position_control/flow_position_control_params.h b/src/examples/flow_position_control/flow_position_control_params.h new file mode 100644 index 000000000..d0c8fc722 --- /dev/null +++ b/src/examples/flow_position_control/flow_position_control_params.h @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file flow_position_control_params.h + * + * Parameters for position controller + */ + +#include <systemlib/param/param.h> + +struct flow_position_control_params { + float pos_p; + float pos_d; + float height_p; + float height_i; + float height_d; + float height_rate; + float height_min; + float height_max; + float thrust_feedforward; + float limit_speed_x; + float limit_speed_y; + float limit_height_error; + float limit_thrust_int; + float limit_thrust_upper; + float limit_thrust_lower; + float limit_yaw_step; + float manual_threshold; + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; +}; + +struct flow_position_control_param_handles { + param_t pos_p; + param_t pos_d; + param_t height_p; + param_t height_i; + param_t height_d; + param_t height_rate; + param_t height_min; + param_t height_max; + param_t thrust_feedforward; + param_t limit_speed_x; + param_t limit_speed_y; + param_t limit_height_error; + param_t limit_thrust_int; + param_t limit_thrust_upper; + param_t limit_thrust_lower; + param_t limit_yaw_step; + param_t manual_threshold; + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct flow_position_control_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p); diff --git a/src/examples/flow_position_control/module.mk b/src/examples/flow_position_control/module.mk new file mode 100644 index 000000000..b10dc490a --- /dev/null +++ b/src/examples/flow_position_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build multirotor position control +# + +MODULE_COMMAND = flow_position_control + +SRCS = flow_position_control_main.c \ + flow_position_control_params.c diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c new file mode 100644 index 000000000..2389c693d --- /dev/null +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -0,0 +1,462 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file flow_position_estimator_main.c + * + * Optical flow position estimator + */ + +#include <nuttx/config.h> +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <stdbool.h> +#include <fcntl.h> +#include <float.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <drivers/drv_hrt.h> +#include <termios.h> +#include <errno.h> +#include <limits.h> +#include <math.h> +#include <uORB/uORB.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/optical_flow.h> +#include <uORB/topics/filtered_bottom_flow.h> +#include <systemlib/perf_counter.h> +#include <poll.h> + +#include "flow_position_estimator_params.h" + +__EXPORT int flow_position_estimator_main(int argc, char *argv[]); +static bool thread_should_exit = false; /**< Daemon exit flag */ +static bool thread_running = false; /**< Daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +int flow_position_estimator_thread_main(int argc, char *argv[]); +static void usage(const char *reason); + +static void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +/** + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int flow_position_estimator_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) + { + if (thread_running) + { + printf("flow position estimator already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn("flow_position_estimator", + SCHED_RR, + SCHED_PRIORITY_MAX - 5, + 4096, + flow_position_estimator_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) + { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) + { + if (thread_running) + printf("\tflow position estimator is running\n"); + else + printf("\tflow position estimator not started\n"); + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int flow_position_estimator_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + thread_running = true; + printf("[flow position estimator] starting\n"); + + /* rotation matrix for transformation of optical flow speed vectors */ + static const int8_t rotM_flow_sensor[3][3] = {{ 0, 1, 0 }, + { -1, 0, 0 }, + { 0, 0, 1 }}; // 90deg rotated + const float time_scale = powf(10.0f,-6.0f); + static float speed[3] = {0.0f, 0.0f, 0.0f}; + static float flow_speed[3] = {0.0f, 0.0f, 0.0f}; + static float global_speed[3] = {0.0f, 0.0f, 0.0f}; + static uint32_t counter = 0; + static uint64_t time_last_flow = 0; // in ms + static float dt = 0.0f; // seconds + static float sonar_last = 0.0f; + static bool sonar_valid = false; + static float sonar_lp = 0.0f; + + /* subscribe to vehicle status, attitude, sensors and flow*/ + struct vehicle_status_s vstatus; + memset(&vstatus, 0, sizeof(vstatus)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); + + /* subscribe to parameter changes */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + + /* subscribe to vehicle status */ + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* subscribe to attitude */ + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + + /* subscribe to attitude setpoint */ + int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + + /* subscribe to optical flow*/ + int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); + + /* init local position and filtered flow struct */ + struct vehicle_local_position_s local_pos = { + .x = 0.0f, + .y = 0.0f, + .z = 0.0f, + .vx = 0.0f, + .vy = 0.0f, + .vz = 0.0f + }; + struct filtered_bottom_flow_s filtered_flow = { + .sumx = 0.0f, + .sumy = 0.0f, + .vx = 0.0f, + .vy = 0.0f + }; + + /* advert pub messages */ + orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); + orb_advert_t filtered_flow_pub = orb_advertise(ORB_ID(filtered_bottom_flow), &filtered_flow); + + /* vehicle flying status parameters */ + bool vehicle_liftoff = false; + bool sensors_ready = false; + + /* parameters init*/ + struct flow_position_estimator_params params; + struct flow_position_estimator_param_handles param_handles; + parameters_init(¶m_handles); + parameters_update(¶m_handles, ¶ms); + + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_estimator_runtime"); + perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval"); + perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err"); + + while (!thread_should_exit) + { +// XXX fix this +#if 0 + if (sensors_ready) + { + /*This runs at the rate of the sensors */ + struct pollfd fds[2] = { + { .fd = optical_flow_sub, .events = POLLIN }, + { .fd = parameter_update_sub, .events = POLLIN } + }; + + /* wait for a sensor update, check for exit condition every 500 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + + } + else if (ret == 0) + { + /* no return value, ignore */ +// printf("[flow position estimator] no bottom flow.\n"); + } + else + { + + /* parameter update available? */ + if (fds[1].revents & POLLIN) + { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + parameters_update(¶m_handles, ¶ms); + printf("[flow position estimator] parameters updated.\n"); + } + + /* only if flow data changed */ + if (fds[0].revents & POLLIN) + { + perf_begin(mc_loop_perf); + + orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + /* got flow, updating attitude and status as well */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + + /* vehicle state estimation */ + float sonar_new = flow.ground_distance_m; + + /* set liftoff boolean + * -> at bottom sonar sometimes does not work correctly, and has to be calibrated (distance higher than 0.3m) + * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time) + * -> minimum sonar value 0.3m + */ + + if (!vehicle_liftoff) + { + if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + vehicle_liftoff = true; + } + else + { + if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + vehicle_liftoff = false; + } + + /* calc dt between flow timestamps */ + /* ignore first flow msg */ + if(time_last_flow == 0) + { + time_last_flow = flow.timestamp; + continue; + } + dt = (float)(flow.timestamp - time_last_flow) * time_scale ; + time_last_flow = flow.timestamp; + + /* only make position update if vehicle is lift off or DEBUG is activated*/ + if (vehicle_liftoff || params.debug) + { + /* copy flow */ + flow_speed[0] = flow.flow_comp_x_m; + flow_speed[1] = flow.flow_comp_y_m; + flow_speed[2] = 0.0f; + + /* convert to bodyframe velocity */ + for(uint8_t i = 0; i < 3; i++) + { + float sum = 0.0f; + for(uint8_t j = 0; j < 3; j++) + sum = sum + flow_speed[j] * rotM_flow_sensor[j][i]; + + speed[i] = sum; + } + + /* update filtered flow */ + filtered_flow.sumx = filtered_flow.sumx + speed[0] * dt; + filtered_flow.sumy = filtered_flow.sumy + speed[1] * dt; + filtered_flow.vx = speed[0]; + filtered_flow.vy = speed[1]; + + // TODO add yaw rotation correction (with distance to vehicle zero) + + /* convert to globalframe velocity + * -> local position is currently not used for position control + */ + for(uint8_t i = 0; i < 3; i++) + { + float sum = 0.0f; + for(uint8_t j = 0; j < 3; j++) + sum = sum + speed[j] * att.R[i][j]; + + global_speed[i] = sum; + } + + local_pos.x = local_pos.x + global_speed[0] * dt; + local_pos.y = local_pos.y + global_speed[1] * dt; + local_pos.vx = global_speed[0]; + local_pos.vy = global_speed[1]; + } + else + { + /* set speed to zero and let position as it is */ + filtered_flow.vx = 0; + filtered_flow.vy = 0; + local_pos.vx = 0; + local_pos.vy = 0; + } + + /* filtering ground distance */ + if (!vehicle_liftoff || !vstatus.flag_system_armed) + { + /* not possible to fly */ + sonar_valid = false; + local_pos.z = 0.0f; + } + else + { + sonar_valid = true; + } + + if (sonar_valid || params.debug) + { + /* simple lowpass sonar filtering */ + /* if new value or with sonar update frequency */ + if (sonar_new != sonar_last || counter % 10 == 0) + { + sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp; + sonar_last = sonar_new; + } + + float height_diff = sonar_new - sonar_lp; + + /* if over 1/2m spike follow lowpass */ + if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold) + { + local_pos.z = -sonar_lp; + } + else + { + local_pos.z = -sonar_new; + } + } + + filtered_flow.timestamp = hrt_absolute_time(); + local_pos.timestamp = hrt_absolute_time(); + + /* publish local position */ + if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z) + && isfinite(local_pos.vx) && isfinite(local_pos.vy)) + { + orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos); + } + + /* publish filtered flow */ + if(isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) && isfinite(filtered_flow.vy)) + { + orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow); + } + + /* measure in what intervals the position estimator runs */ + perf_count(mc_interval_perf); + perf_end(mc_loop_perf); + + } + } + + } + else + { + /* sensors not ready waiting for first attitude msg */ + + /* polling */ + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + /* wait for a attitude message, check for exit condition every 5 s */ + int ret = poll(fds, 1, 5000); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } + else if (ret == 0) + { + /* no return value, ignore */ + printf("[flow position estimator] no attitude received.\n"); + } + else + { + if (fds[0].revents & POLLIN){ + sensors_ready = true; + printf("[flow position estimator] initialized.\n"); + } + } + } + + counter++; + #endif + } + + printf("[flow position estimator] exiting.\n"); + thread_running = false; + + close(vehicle_attitude_setpoint_sub); + close(vehicle_attitude_sub); + close(vehicle_status_sub); + close(parameter_update_sub); + close(optical_flow_sub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + + fflush(stdout); + return 0; +} + + diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.c b/src/examples/flow_position_estimator/flow_position_estimator_params.c new file mode 100644 index 000000000..ec3c3352d --- /dev/null +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.c @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file flow_position_estimator_params.c + * + * Parameters for position estimator + */ + +#include "flow_position_estimator_params.h" + +/* Extended Kalman Filter covariances */ + +/* controller parameters */ +PARAM_DEFINE_FLOAT(FPE_LO_THRUST, 0.4f); +PARAM_DEFINE_FLOAT(FPE_SONAR_LP_U, 0.5f); +PARAM_DEFINE_FLOAT(FPE_SONAR_LP_L, 0.2f); +PARAM_DEFINE_INT32(FPE_DEBUG, 0); + + +int parameters_init(struct flow_position_estimator_param_handles *h) +{ + /* PID parameters */ + h->minimum_liftoff_thrust = param_find("FPE_LO_THRUST"); + h->sonar_upper_lp_threshold = param_find("FPE_SONAR_LP_U"); + h->sonar_lower_lp_threshold = param_find("FPE_SONAR_LP_L"); + h->debug = param_find("FPE_DEBUG"); + + return OK; +} + +int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p) +{ + param_get(h->minimum_liftoff_thrust, &(p->minimum_liftoff_thrust)); + param_get(h->sonar_upper_lp_threshold, &(p->sonar_upper_lp_threshold)); + param_get(h->sonar_lower_lp_threshold, &(p->sonar_lower_lp_threshold)); + param_get(h->debug, &(p->debug)); + + return OK; +} diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.h b/src/examples/flow_position_estimator/flow_position_estimator_params.h new file mode 100644 index 000000000..f9a9bb303 --- /dev/null +++ b/src/examples/flow_position_estimator/flow_position_estimator_params.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file flow_position_estimator_params.h + * + * Parameters for position estimator + */ + +#include <systemlib/param/param.h> + +struct flow_position_estimator_params { + float minimum_liftoff_thrust; + float sonar_upper_lp_threshold; + float sonar_lower_lp_threshold; + int debug; +}; + +struct flow_position_estimator_param_handles { + param_t minimum_liftoff_thrust; + param_t sonar_upper_lp_threshold; + param_t sonar_lower_lp_threshold; + param_t debug; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct flow_position_estimator_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p); diff --git a/src/examples/flow_position_estimator/module.mk b/src/examples/flow_position_estimator/module.mk new file mode 100644 index 000000000..88c9ceb93 --- /dev/null +++ b/src/examples/flow_position_estimator/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build position estimator +# + +MODULE_COMMAND = flow_position_estimator + +SRCS = flow_position_estimator_main.c \ + flow_position_estimator_params.c diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c new file mode 100644 index 000000000..0be4b3d5a --- /dev/null +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -0,0 +1,363 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file flow_speed_control.c + * + * Optical flow speed controller + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <termios.h> +#include <time.h> +#include <math.h> +#include <sys/prctl.h> +#include <drivers/drv_hrt.h> +#include <uORB/uORB.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h> +#include <uORB/topics/filtered_bottom_flow.h> +#include <systemlib/systemlib.h> +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <poll.h> + +#include "flow_speed_control_params.h" + + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +__EXPORT int flow_speed_control_main(int argc, char *argv[]); + +/** + * Mainloop of position controller. + */ +static int flow_speed_control_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_spawn(). + */ +int flow_speed_control_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) + { + if (thread_running) + { + printf("flow speed control already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("flow_speed_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 6, + 4096, + flow_speed_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) + { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) + { + if (thread_running) + printf("\tflow speed control app is running\n"); + else + printf("\tflow speed control app not started\n"); + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +static int +flow_speed_control_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + thread_running = true; + printf("[flow speed control] starting\n"); + + uint32_t counter = 0; + + /* structures */ + struct vehicle_status_s vstatus; + struct filtered_bottom_flow_s filtered_flow; + struct vehicle_bodyframe_speed_setpoint_s speed_sp; + + struct vehicle_attitude_setpoint_s att_sp; + + /* subscribe to attitude, motor setpoints and system state */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); + int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); + + orb_advert_t att_sp_pub; + bool attitude_setpoint_adverted = false; + + /* parameters init*/ + struct flow_speed_control_params params; + struct flow_speed_control_param_handles param_handles; + parameters_init(¶m_handles); + parameters_update(¶m_handles, ¶ms); + + /* register the perf counter */ + perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_speed_control_runtime"); + perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_speed_control_interval"); + perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err"); + + static bool sensors_ready = false; + + while (!thread_should_exit) + { +#if 0 + /* wait for first attitude msg to be sure all data are available */ + if (sensors_ready) + { + /* polling */ + struct pollfd fds[2] = { + { .fd = vehicle_bodyframe_speed_setpoint_sub, .events = POLLIN }, // speed setpoint from pos controller + { .fd = parameter_update_sub, .events = POLLIN } + }; + + /* wait for a position update, check for exit condition every 5000 ms */ + int ret = poll(fds, 2, 500); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } + else if (ret == 0) + { + /* no return value, ignore */ +// printf("[flow speed control] no bodyframe speed setpoints updates\n"); + } + else + { + /* parameter update available? */ + if (fds[1].revents & POLLIN) + { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + parameters_update(¶m_handles, ¶ms); + printf("[flow speed control] parameters updated.\n"); + } + + /* only run controller if position/speed changed */ + if (fds[0].revents & POLLIN) + { + perf_begin(mc_loop_perf); + + /* get a local copy of the vehicle state */ + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + /* get a local copy of filtered bottom flow */ + orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); + /* get a local copy of bodyframe speed setpoint */ + orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp); + + if (vstatus.state_machine == SYSTEM_STATE_AUTO) + { + /* calc new roll/pitch */ + float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; + float roll_body = (speed_sp.vy - filtered_flow.vy) * params.speed_p; + + /* limit roll and pitch corrections */ + if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch)) + { + att_sp.pitch_body = pitch_body; + } + else + { + if(pitch_body > params.limit_pitch) + att_sp.pitch_body = params.limit_pitch; + if(pitch_body < -params.limit_pitch) + att_sp.pitch_body = -params.limit_pitch; + } + + if((roll_body <= params.limit_roll) && (roll_body >= -params.limit_roll)) + { + att_sp.roll_body = roll_body; + } + else + { + if(roll_body > params.limit_roll) + att_sp.roll_body = params.limit_roll; + if(roll_body < -params.limit_roll) + att_sp.roll_body = -params.limit_roll; + } + + /* set yaw setpoint forward*/ + att_sp.yaw_body = speed_sp.yaw_sp; + + /* add trim from parameters */ + att_sp.roll_body = att_sp.roll_body + params.trim_roll; + att_sp.pitch_body = att_sp.pitch_body + params.trim_pitch; + + att_sp.thrust = speed_sp.thrust_sp; + att_sp.timestamp = hrt_absolute_time(); + + /* publish new attitude setpoint */ + if(isfinite(att_sp.pitch_body) && isfinite(att_sp.roll_body) && isfinite(att_sp.yaw_body) && isfinite(att_sp.thrust)) + { + if (attitude_setpoint_adverted) + { + 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); + attitude_setpoint_adverted = true; + } + } + else + { + warnx("NaN in flow speed controller!"); + } + } + else + { + /* reset attitude setpoint */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.thrust = 0.0f; + att_sp.yaw_body = 0.0f; + } + + /* measure in what intervals the controller runs */ + perf_count(mc_interval_perf); + perf_end(mc_loop_perf); + } + } + + counter++; + } + else + { + /* sensors not ready waiting for first attitude msg */ + + /* polling */ + struct pollfd fds[1] = { + { .fd = vehicle_attitude_sub, .events = POLLIN }, + }; + + /* wait for a flow msg, check for exit condition every 5 s */ + int ret = poll(fds, 1, 5000); + + if (ret < 0) + { + /* poll error, count it in perf */ + perf_count(mc_err_perf); + } + else if (ret == 0) + { + /* no return value, ignore */ + printf("[flow speed control] no attitude received.\n"); + } + else + { + if (fds[0].revents & POLLIN) + { + sensors_ready = true; + printf("[flow speed control] initialized.\n"); + } + } + } +#endif + } + + printf("[flow speed control] ending now...\n"); + + thread_running = false; + + close(parameter_update_sub); + close(vehicle_attitude_sub); + close(vehicle_bodyframe_speed_setpoint_sub); + close(filtered_bottom_flow_sub); + close(vehicle_status_sub); + close(att_sp_pub); + + perf_print_counter(mc_loop_perf); + perf_free(mc_loop_perf); + + fflush(stdout); + return 0; +} diff --git a/src/examples/flow_speed_control/flow_speed_control_params.c b/src/examples/flow_speed_control/flow_speed_control_params.c new file mode 100644 index 000000000..8dfe54173 --- /dev/null +++ b/src/examples/flow_speed_control/flow_speed_control_params.c @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file flow_speed_control_params.c + * + */ + +#include "flow_speed_control_params.h" + +/* controller parameters */ +PARAM_DEFINE_FLOAT(FSC_S_P, 0.1f); +PARAM_DEFINE_FLOAT(FSC_L_PITCH, 0.4f); +PARAM_DEFINE_FLOAT(FSC_L_ROLL, 0.4f); + +int parameters_init(struct flow_speed_control_param_handles *h) +{ + /* PID parameters */ + h->speed_p = param_find("FSC_S_P"); + h->limit_pitch = param_find("FSC_L_PITCH"); + h->limit_roll = param_find("FSC_L_ROLL"); + h->trim_roll = param_find("TRIM_ROLL"); + h->trim_pitch = param_find("TRIM_PITCH"); + + + return OK; +} + +int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p) +{ + param_get(h->speed_p, &(p->speed_p)); + param_get(h->limit_pitch, &(p->limit_pitch)); + param_get(h->limit_roll, &(p->limit_roll)); + param_get(h->trim_roll, &(p->trim_roll)); + param_get(h->trim_pitch, &(p->trim_pitch)); + + return OK; +} diff --git a/src/examples/flow_speed_control/flow_speed_control_params.h b/src/examples/flow_speed_control/flow_speed_control_params.h new file mode 100644 index 000000000..eec27a2bf --- /dev/null +++ b/src/examples/flow_speed_control/flow_speed_control_params.h @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file flow_speed_control_params.h + * + * Parameters for speed controller + */ + +#include <systemlib/param/param.h> + +struct flow_speed_control_params { + float speed_p; + float limit_pitch; + float limit_roll; + float trim_roll; + float trim_pitch; +}; + +struct flow_speed_control_param_handles { + param_t speed_p; + param_t limit_pitch; + param_t limit_roll; + param_t trim_roll; + param_t trim_pitch; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct flow_speed_control_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p); diff --git a/src/examples/flow_speed_control/module.mk b/src/examples/flow_speed_control/module.mk new file mode 100644 index 000000000..5a4182146 --- /dev/null +++ b/src/examples/flow_speed_control/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Build flow speed control +# + +MODULE_COMMAND = flow_speed_control + +SRCS = flow_speed_control_main.c \ + flow_speed_control_params.c 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 8e18c3c9a..16d5ad626 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -126,7 +126,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 12400, + 14000, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 7d3812abd..52dac652b 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,42 +44,42 @@ /* Extended Kalman Filter covariances */ /* gyro process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); /* gyro offsets process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f); /* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f); /* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); +PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); +PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ - h->q0 = param_find("EKF_ATT_V2_Q0"); - h->q1 = param_find("EKF_ATT_V2_Q1"); - h->q2 = param_find("EKF_ATT_V2_Q2"); - h->q3 = param_find("EKF_ATT_V2_Q3"); - h->q4 = param_find("EKF_ATT_V2_Q4"); + h->q0 = param_find("EKF_ATT_V3_Q0"); + h->q1 = param_find("EKF_ATT_V3_Q1"); + h->q2 = param_find("EKF_ATT_V3_Q2"); + h->q3 = param_find("EKF_ATT_V3_Q3"); + h->q4 = param_find("EKF_ATT_V3_Q4"); - h->r0 = param_find("EKF_ATT_V2_R0"); - h->r1 = param_find("EKF_ATT_V2_R1"); - h->r2 = param_find("EKF_ATT_V2_R2"); - h->r3 = param_find("EKF_ATT_V2_R3"); + h->r0 = param_find("EKF_ATT_V3_R0"); + h->r1 = param_find("EKF_ATT_V3_R1"); + h->r2 = param_find("EKF_ATT_V3_R2"); + h->r3 = param_find("EKF_ATT_V3_R3"); - h->roll_off = param_find("ATT_ROLL_OFFS"); - h->pitch_off = param_find("ATT_PITCH_OFFS"); - h->yaw_off = param_find("ATT_YAW_OFFS"); + h->roll_off = param_find("ATT_ROLL_OFF3"); + h->pitch_off = param_find("ATT_PITCH_OFF3"); + h->yaw_off = param_find("ATT_YAW_OFF3"); return OK; } diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index d79dd93dd..231739962 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -78,24 +78,21 @@ #include <systemlib/conversions.h> #include <mavlink/mavlink_log.h> -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); -int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +void do_accel_calibration(int mavlink_fd); +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); int mat_invert3(float src[3][3], float dst[3][3]); int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { +void do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); - /* set to accel calibration mode */ - status->flag_preflight_accel_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_scale[3]; - int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale); + int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); if (res == OK) { /* measurements complete successfully, set parameters */ @@ -131,24 +128,17 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m } mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ + tune_positive(); } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); - tune_error(); - sleep(2); + tune_negative(); } /* exit accel calibration mode */ - status->flag_preflight_accel_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); } -int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; @@ -198,7 +188,7 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); mavlink_log_info(mavlink_fd, str); data_collected[orient] = true; - tune_confirm(); + tune_neutral(); } close(sensor_combined_sub); diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index a11cf93d3..6a920c4ef 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -11,6 +11,6 @@ #include <stdint.h> #include <uORB/topics/vehicle_status.h> -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); +void do_accel_calibration(int mavlink_fd); #endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 937c515e6..edcdf7e54 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -60,13 +60,9 @@ #include <debug.h> #include <sys/prctl.h> #include <string.h> -#include <drivers/drv_led.h> -#include <drivers/drv_hrt.h> -#include <drivers/drv_tone_alarm.h> -#include "state_machine_helper.h" -#include "systemlib/systemlib.h" #include <math.h> #include <poll.h> + #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/battery_status.h> @@ -77,18 +73,27 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_command.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/subsystem_info.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_safety.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/differential_pressure.h> #include <uORB/topics/safety.h> #include <mavlink/mavlink_log.h> +#include <drivers/drv_led.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_tone_alarm.h> + +#include <mavlink/mavlink_log.h> #include <systemlib/param/param.h> #include <systemlib/systemlib.h> #include <systemlib/err.h> +#include <systemlib/cpuload.h> + +#include "state_machine_helper.h" -/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ #include <drivers/drv_accel.h> #include <drivers/drv_gyro.h> #include <drivers/drv_mag.h> @@ -103,7 +108,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); -#include <systemlib/cpuload.h> + extern struct system_load_s system_load; /* Decouple update interval and hysteris counters, all depends on intervals */ @@ -122,25 +127,39 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ + /* File descriptors */ static int leds; static int buzzer; static int mavlink_fd; -static bool commander_initialized = false; -static struct vehicle_status_s current_status; /**< Main state machine */ -static orb_advert_t stat_pub; - -// static uint16_t nofix_counter = 0; -// static uint16_t gotfix_counter = 0; - -static unsigned int failsafe_lowlevel_timeout_ms; +/* flags */ +static bool commander_initialized = false; static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ +/* timout until lowlevel failsafe */ +static unsigned int failsafe_lowlevel_timeout_ms; + +/* tasks waiting for low prio thread */ +enum { + LOW_PRIO_TASK_NONE = 0, + LOW_PRIO_TASK_PARAM_SAVE, + LOW_PRIO_TASK_PARAM_LOAD, + LOW_PRIO_TASK_GYRO_CALIBRATION, + LOW_PRIO_TASK_MAG_CALIBRATION, + LOW_PRIO_TASK_ALTITUDE_CALIBRATION, + LOW_PRIO_TASK_RC_CALIBRATION, + LOW_PRIO_TASK_ACCEL_CALIBRATION, + LOW_PRIO_TASK_AIRSPEED_CALIBRATION +} low_prio_task; + + /* pthread loops */ -static void *orb_receive_loop(void *arg); +void *orb_receive_loop(void *arg); +void *commander_low_prio_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -149,17 +168,25 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -static int buzzer_init(void); -static void buzzer_deinit(void); -static int led_init(void); -static void led_deinit(void); -static int led_toggle(int led); -static int led_on(int led); -static int led_off(int led); -static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); -static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); -static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); -static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); +int buzzer_init(void); +void buzzer_deinit(void); +int led_init(void); +void led_deinit(void); +int led_toggle(int led); +int led_on(int led); +int led_off(int led); +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +void do_reboot(void); +void do_gyro_calibration(void); +void do_mag_calibration(void); +void do_rc_calibration(void); +void do_airspeed_calibration(void); + + +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -168,7 +195,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u /** * Print the correct usage. */ -static void usage(const char *reason); +void usage(const char *reason); /** * Sort calibration values. @@ -180,7 +207,7 @@ static void usage(const char *reason); */ // static void cal_bsort(float a[], int n); -static int buzzer_init() +int buzzer_init() { buzzer = open("/dev/tone_alarm", O_WRONLY); @@ -192,13 +219,13 @@ static int buzzer_init() return 0; } -static void buzzer_deinit() +void buzzer_deinit() { close(buzzer); } -static int led_init() +int led_init() { leds = open(LED_DEVICE_PATH, 0); @@ -215,12 +242,12 @@ static int led_init() return 0; } -static void led_deinit() +void led_deinit() { close(leds); } -static int led_toggle(int led) +int led_toggle(int led) { static int last_blue = LED_ON; static int last_amber = LED_ON; @@ -232,68 +259,61 @@ static int led_toggle(int led) return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); } -static int led_on(int led) +int led_on(int led) { return ioctl(leds, LED_ON, led); } -static int led_off(int led) +int led_off(int led) { return ioctl(leds, LED_OFF, led); } -enum AUDIO_PATTERN { - AUDIO_PATTERN_ERROR = 2, - AUDIO_PATTERN_NOTIFY_POSITIVE = 3, - AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, - AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, - AUDIO_PATTERN_NOTIFY_CHARGE = 6 -}; -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) +void tune_error() { - - /* Trigger alarm if going into any error state */ - if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || - ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); - } - - /* Trigger neutral on arming / disarming */ - if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); - } - - /* Trigger Tetris on being bored */ - - return 0; + ioctl(buzzer, TONE_SET_ALARM, 2); } -void tune_confirm(void) +void tune_positive() { ioctl(buzzer, TONE_SET_ALARM, 3); } -void tune_error(void) +void tune_neutral() { ioctl(buzzer, TONE_SET_ALARM, 4); } -void do_rc_calibration(int status_pub, struct vehicle_status_s *status) +void tune_negative() { - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - return; - } + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +void do_reboot() +{ + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ +} + + +void do_rc_calibration() +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + + /* XXX fix this */ + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp; orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ - float p = sp.roll; param_set(param_find("TRIM_ROLL"), &p); p = sp.pitch; @@ -302,22 +322,21 @@ void do_rc_calibration(int status_pub, struct vehicle_status_s *status) param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ - /* auto-save to EEPROM */ + /* auto-save */ int save_ret = param_save_default(); if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); } + tune_positive(); + mavlink_log_info(mavlink_fd, "trim calibration done"); } -void do_mag_calibration(int status_pub, struct vehicle_status_s *status) +void do_mag_calibration() { - - /* set to mag calibration mode */ - status->flag_preflight_mag_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); struct mag_report mag; @@ -332,15 +351,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* limit update rate to get equally spaced measurements over time (in ms) */ orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - // XXX old cal - // * FLT_MIN is not the most negative float number, - // * but the smallest number by magnitude float can - // * represent. Use -FLT_MAX to initialize the most - // * negative number - - // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); /* erase old calibration */ @@ -386,10 +396,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) return; } - tune_confirm(); - sleep(2); - tune_confirm(); - while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -403,9 +409,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) axis_index++; char buf[50]; - sprintf(buf, "Please rotate around %c", axislabels[axis_index]); + sprintf(buf, "please rotate around %c", axislabels[axis_index]); mavlink_log_info(mavlink_fd, buf); - tune_confirm(); + tune_neutral(); axis_deadline += calibration_interval / 3; } @@ -541,28 +547,19 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "mag calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); + tune_positive(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); } - /* disable calibration mode */ - status->flag_preflight_mag_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - close(sub_mag); } -void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) +void do_gyro_calibration() { - /* set to gyro calibration mode */ - status->flag_preflight_gyro_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); const int calibration_count = 5000; @@ -613,10 +610,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) gyro_offset[1] = gyro_offset[1] / calibration_count; gyro_offset[2] = gyro_offset[2] / calibration_count; - /* exit gyro calibration mode */ - status->flag_preflight_gyro_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) @@ -653,10 +646,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); + tune_positive(); /* third beep by cal end routine */ } else { @@ -666,14 +656,11 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } -void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) -{ - /* announce change */ - mavlink_log_info(mavlink_fd, "keep it still"); - /* set to accel calibration mode */ - status->flag_preflight_airspeed_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); +void do_airspeed_calibration() +{ + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); const int calibration_count = 2500; @@ -722,340 +709,277 @@ void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ + tune_positive(); } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); } - /* exit airspeed calibration mode */ - status->flag_preflight_airspeed_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - close(diff_pres_sub); } - - -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command handling */ - tune_confirm(); - - - /* supported command handling start */ - /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + case VEHICLE_CMD_DO_SET_MODE: - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - break; + /* request to activate HIL */ + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - /* request to arm */ - if ((int)cmd->param1 == 1) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { result = VEHICLE_CMD_RESULT_DENIED; } + } - /* request to disarm */ - - } else if ((int)cmd->param1 == 0) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { result = VEHICLE_CMD_RESULT_DENIED; } - } - } - break; - - /* request for an autopilot reboot */ - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - if ((int)cmd->param1 == 1) { - if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + } else { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - /* system may return here */ result = VEHICLE_CMD_RESULT_DENIED; } } - } - break; -// /* request to land */ -// case VEHICLE_CMD_NAV_LAND: -// { -// //TODO: add check if landing possible -// //TODO: add landing maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } } -// break; -// -// /* request to takeoff */ -// case VEHICLE_CMD_NAV_TAKEOFF: -// { -// //TODO: add check if takeoff possible -// //TODO: add takeoff maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } -// } -// break; -// - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; + break; - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); result = VEHICLE_CMD_RESULT_DENIED; } - - handled = true; - } - - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); result = VEHICLE_CMD_RESULT_DENIED; } - - handled = true; } - /* zero-altitude pressure calibration */ - if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + break; - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - tune_confirm(); + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - } else { - mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - result = VEHICLE_CMD_RESULT_DENIED; - } + /* request for an autopilot reboot */ + if ((int)cmd->param1 == 1) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) { + /* reboot is executed later, after positive tune is sent */ + result = VEHICLE_CMD_RESULT_ACCEPTED; + tune_positive(); + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + do_reboot(); - handled = true; + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + tune_negative(); + } + } } + break; - /* trim calibration */ - if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting trim cal"); - tune_confirm(); - do_rc_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished trim cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // /* request to land */ + // case VEHICLE_CMD_NAV_LAND: + // { + // //TODO: add check if landing possible + // //TODO: add landing maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } } + // break; + // + // /* request to takeoff */ + // case VEHICLE_CMD_NAV_TAKEOFF: + // { + // //TODO: add check if takeoff possible + // //TODO: add takeoff maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } + // } + // break; + // + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: - handled = true; - } + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "CMD starting accel cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status, mavlink_fd); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; } - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "CMD starting airspeed cal"); - tune_confirm(); - do_airspeed_calibration(status_pub, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; } - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } - } - break; - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + // /* zero-altitude pressure calibration */ + // if ((int)(cmd->param3) == 1) { - } else { + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } - if (((int)(cmd->param1)) == 0) { - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* trim calibration */ + // if ((int)(cmd->param4) == 1) { - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - result = VEHICLE_CMD_RESULT_FAILED; + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } - } else if (((int)(cmd->param1)) == 1) { + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); - mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; - } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } + case VEHICLE_CMD_PREFLIGHT_STORAGE: - result = VEHICLE_CMD_RESULT_FAILED; - } + if (((int)(cmd->param1)) == 0) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + + } else if (((int)(cmd->param1)) == 1) { + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } - } break; - default: { - mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command rejection */ - ioctl(buzzer, TONE_SET_ALARM, 4); - } + default: + mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; break; } /* supported command handling stop */ if (result == VEHICLE_CMD_RESULT_FAILED || result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - ioctl(buzzer, TONE_SET_ALARM, 5); + result == VEHICLE_CMD_RESULT_UNSUPPORTED || + result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + + tune_negative(); } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_confirm(); + + tune_positive(); } /* send any requested ACKs */ @@ -1066,7 +990,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal +void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal { /* Set thread name */ prctl(PR_SET_NAME, "commander orb rcv", getpid()); @@ -1169,8 +1093,7 @@ float battery_remaining_estimate_voltage(float voltage) return ret; } -static void -usage(const char *reason) +void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -1245,62 +1168,100 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_component_id = param_find("MAV_COMP_ID"); /* welcome user */ - warnx("I am in command now!\n"); + warnx("[commander] starting"); /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; pthread_t subsystem_info_thread; + pthread_t commander_low_prio_thread; /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds\n"); + warnx("ERROR: Failed to initialize leds"); } if (buzzer_init() != 0) { - warnx("ERROR: Failed to initialize buzzer\n"); + warnx("ERROR: Failed to initialize buzzer"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } - + + /* Main state machine */ + struct vehicle_status_s current_status; + orb_advert_t status_pub; /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.state_machine = SYSTEM_STATE_PREFLIGHT; - current_status.flag_system_armed = false; + + /* safety topic */ + struct actuator_safety_s safety; + orb_advert_t safety_pub; + /* Initialize safety with all false */ + memset(&safety, 0, sizeof(safety)); + + + /* flags for control apps */ + struct vehicle_control_mode_s control_mode; + orb_advert_t control_mode_pub; + + /* Initialize all flags to false */ + memset(&control_mode, 0, sizeof(control_mode)); + + current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.arming_state = ARMING_STATE_INIT; + current_status.hil_state = HIL_STATE_OFF; + /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; current_status.rc_signal_found_once = false; + /* mark all signals lost as long as they haven't been found */ current_status.rc_signal_lost = true; current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ - current_status.flag_vector_flight_mode_ok = false; + // current_status.flag_vector_flight_mode_ok = false; + /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; /* set safety device detection flag */ - current_status.flag_safety_present = false; + + /* XXX do we need this? */ + //current_status.flag_safety_present = false; + + // XXX for now just set sensors as initialized + current_status.condition_system_sensors_initialized = true; + + // XXX just disable offboard control for now + control_mode.flag_control_offboard_enabled = false; /* advertise to ORB */ - stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ - state_machine_publish(stat_pub, ¤t_status, mavlink_fd); + state_machine_publish(status_pub, ¤t_status, mavlink_fd); + + safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); + + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; memset(&home, 0, sizeof(home)); - if (stat_pub < 0) { + if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); exit(ERROR); } + // XXX needed? mavlink_log_info(mavlink_fd, "system is running"); /* create pthreads */ @@ -1309,9 +1270,18 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_setstacksize(&subsystem_info_attr, 2048); pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + + struct sched_param param; + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + /* Start monitoring loop */ uint16_t counter = 0; - uint8_t flight_env; /* Initialize to 0.0V */ float battery_voltage = 0.0f; @@ -1320,12 +1290,14 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; uint8_t low_voltage_counter = 0; uint16_t critical_voltage_counter = 0; - int16_t mode_switch_rc_value; float bat_remain = 1.0f; uint16_t stick_off_counter = 0; uint16_t stick_on_counter = 0; + /* XXX what exactly is this for? */ + uint64_t last_print_time = 0; + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -1336,11 +1308,13 @@ int commander_thread_main(int argc, char *argv[]) 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)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); uint64_t last_global_position_time = 0; + /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); @@ -1351,10 +1325,13 @@ int commander_thread_main(int argc, char *argv[]) * position estimator and commander. RAW GPS is more than good enough for a * non-flying vehicle. */ + + /* Subscribe to GPS topic */ int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); + /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); @@ -1380,12 +1357,6 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; - /* subscribe to safety topic */ - int safety_sub = orb_subscribe(ORB_ID(safety)); - struct safety_s safety; - memset(&safety, 0, sizeof(safety)); - safety.status = SAFETY_STATUS_NOT_PRESENT; - // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1396,7 +1367,9 @@ int commander_thread_main(int argc, char *argv[]) thread_running = true; uint64_t start_time = hrt_absolute_time(); - uint64_t failsave_ll_start_time = 0; + + /* XXX use this! */ + //uint64_t failsave_ll_start_time = 0; bool state_changed = true; bool param_init_forced = true; @@ -1416,7 +1389,7 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ - if (!current_status.flag_system_armed) { + if (!safety.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1470,20 +1443,38 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(stat_pub, ¤t_status, &cmd); + handle_command(status_pub, ¤t_status, &cmd, safety_pub, &safety); } + - orb_check(safety_sub, &new_data); + /* update parameters */ + orb_check(param_changed_sub, &new_data); - if (new_data) { - /* got safety change */ - orb_copy(ORB_ID(safety), safety_sub, &safety); + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - /* handle it */ - current_status.flag_safety_present = (safety.status != SAFETY_STATUS_NOT_PRESENT); - - if (current_status.flag_safety_present) - current_status.flag_safety_safe = (safety.status == SAFETY_STATUS_SAFE); + /* update parameters */ + if (!safety.armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + + } else { + current_status.flag_external_manual_override_ok = true; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + } } /* update global position estimate */ @@ -1504,6 +1495,13 @@ int commander_thread_main(int argc, char *argv[]) last_local_position_time = local_position.timestamp; } + /* set the condition to valid if there has recently been a local position estimate */ + if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { + current_status.condition_local_position_valid = true; + } else { + current_status.condition_local_position_valid = false; + } + /* update battery status */ orb_check(battery_sub, &new_data); if (new_data) { @@ -1522,10 +1520,11 @@ int commander_thread_main(int argc, char *argv[]) /* Slow but important 8 Hz checks */ if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { - /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ - if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || + + /* XXX if armed */ + if ((false /*current_status.state_machine == SYSTEM_STATE_GROUND_READY || current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL)) { + current_status.state_machine == SYSTEM_STATE_MANUAL*/)) { /* armed, solid */ led_on(LED_AMBER); @@ -1560,26 +1559,18 @@ int commander_thread_main(int argc, char *argv[]) } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { /* toggle arming (red) at 5 Hz on low battery or error */ led_toggle(LED_AMBER); - - } else { - // /* Constant error indication in standby mode without GPS */ - // if (!current_status.gps_valid) { - // led_on(LED_AMBER); - - // } else { - // led_off(LED_AMBER); - // } } - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + } - if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - last_idle_time = system_load.tasks[0].total_runtime; - } + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + + last_idle_time = system_load.tasks[0].total_runtime; } // // XXX Export patterns and threshold to parameters @@ -1595,7 +1586,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { /* For less than 20%, start be slightly annoying at 1 Hz */ ioctl(buzzer, TONE_SET_ALARM, 0); - tune_confirm(); + tune_positive(); } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { ioctl(buzzer, TONE_SET_ALARM, 0); @@ -1622,13 +1613,14 @@ int commander_thread_main(int argc, char *argv[]) low_voltage_counter++; } - /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ + /* Critical, this is rather an emergency, change state machine */ else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + // XXX implement this + // state_machine_emergency(status_pub, ¤t_status, mavlink_fd); } critical_voltage_counter++; @@ -1640,6 +1632,14 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + /* If in INIT state, try to proceed to STANDBY state */ + if (current_status.arming_state == ARMING_STATE_INIT) { + // XXX check for sensors + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + } else { + // XXX: Add emergency stuff if sensors are lost + } + /* * Check for valid position information. @@ -1651,53 +1651,53 @@ int commander_thread_main(int argc, char *argv[]) */ /* store current state to reason later about a state change */ - bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.flag_global_position_valid; - bool local_pos_valid = current_status.flag_local_position_valid; - bool airspeed_valid = current_status.flag_airspeed_valid; + // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.condition_global_position_valid; + bool local_pos_valid = current_status.condition_local_position_valid; + bool airspeed_valid = current_status.condition_airspeed_valid; + /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { - current_status.flag_global_position_valid = true; + current_status.condition_global_position_valid = true; // XXX check for controller status and home position as well } else { - current_status.flag_global_position_valid = false; + current_status.condition_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_local_position_valid = true; + current_status.condition_local_position_valid = true; // XXX check for controller status and home position as well } else { - current_status.flag_local_position_valid = false; + current_status.condition_local_position_valid = false; } /* Check for valid airspeed/differential pressure measurements */ if (hrt_absolute_time() - last_diff_pres_time < 2000000) { - current_status.flag_airspeed_valid = true; + current_status.condition_airspeed_valid = true; } else { - current_status.flag_airspeed_valid = false; + current_status.condition_airspeed_valid = false; } /* * Consolidate global position and local position valid flags * for vector flight mode. */ - if (current_status.flag_local_position_valid || - current_status.flag_global_position_valid) { - current_status.flag_vector_flight_mode_ok = true; + // if (current_status.condition_local_position_valid || + // current_status.condition_global_position_valid) { + // current_status.flag_vector_flight_mode_ok = true; - } else { - current_status.flag_vector_flight_mode_ok = false; - } + // } else { + // current_status.flag_vector_flight_mode_ok = false; + // } /* consolidate state change, flag as changed if required */ - if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || - global_pos_valid != current_status.flag_global_position_valid || - local_pos_valid != current_status.flag_local_position_valid || - airspeed_valid != current_status.flag_airspeed_valid) { + if (global_pos_valid != current_status.condition_global_position_valid || + local_pos_valid != current_status.condition_local_position_valid || + airspeed_valid != current_status.condition_airspeed_valid) { state_changed = true; } @@ -1711,20 +1711,21 @@ int commander_thread_main(int argc, char *argv[]) * 2) The system has not aquired position lock before * 3) The system is not armed (on the ground) */ - if (!current_status.flag_valid_launch_position && - !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - !current_status.flag_system_armed) { - /* first time a valid position, store it and emit it */ - - // XXX implement storage and publication of RTL position - current_status.flag_valid_launch_position = true; - current_status.flag_auto_flight_mode_ok = true; - state_changed = true; - } + // if (!current_status.flag_valid_launch_position && + // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + // !current_status.flag_system_armed) { + // first time a valid position, store it and emit it + + // // XXX implement storage and publication of RTL position + // current_status.flag_valid_launch_position = true; + // current_status.flag_auto_flight_mode_ok = true; + // state_changed = true; + // } orb_check(ORB_ID(vehicle_gps_position), &new_data); if (new_data) { + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); /* check for first, long-term and valid GPS lock -> set home position */ @@ -1748,10 +1749,10 @@ int commander_thread_main(int argc, char *argv[]) if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_system_armed) { + && !safety.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1774,7 +1775,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ home_position_set = true; - tune_confirm(); + tune_positive(); } } @@ -1784,100 +1785,266 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - // /* - // * Check if manual control modes have to be switched - // */ - // if (!isfinite(sp_man.manual_mode_switch)) { - // warnx("man mode sw not finite\n"); - - // /* this switch is not properly mapped, set default */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, fall back to direct pass-through */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { - // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { + warnx("mode sw not finite"); + /* no valid stick position, go to default */ + current_status.mode_switch = MODE_SWITCH_MANUAL; - // /* bottom stick position, set direct mode for vehicles supporting it */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { + /* bottom stick position, go to manual mode */ + current_status.mode_switch = MODE_SWITCH_MANUAL; - // /* assuming a fixed wing, set to direct pass-through as requested */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { + /* top stick position, set auto/mission for all vehicle types */ + current_status.mode_switch = MODE_SWITCH_AUTO; - // /* top stick position, set SAS for all vehicle types */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; + } else { - // } else { - // /* center stick position, set rate control */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = true; - // } + /* center stick position, set seatbelt/simple control */ + current_status.mode_switch = MODE_SWITCH_SEATBELT; + } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* - * Check if manual stability control modes have to be switched - */ - if (!isfinite(sp_man.manual_sas_switch)) { + * Check if land/RTL is requested + */ + if (!isfinite(sp_man.return_switch)) { /* this switch is not properly mapped, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + current_status.return_switch = RETURN_SWITCH_RETURN; } else { /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.return_switch = RETURN_SWITCH_NONE; + } + + /* check mission switch */ + if (!isfinite(sp_man.mission_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + + /* top switch position */ + current_status.mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom switch position */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else { + + /* center switch position, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + } + + /* Now it's time to handle the stick inputs */ + + switch (current_status.arming_state) { + + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: + + /* just manual, XXX this might depend on the return switch */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + + /* Try seatbelt or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + + /* Try auto or fallback to seatbelt or even manual */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // first fallback to SEATBELT_STANDY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // or fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + } + } + + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + + /* SEATBELT_STANDBY (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_NONE) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* SEATBELT_DESCENT (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_RETURN) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_NONE) { + + /* we might come from the disarmed state AUTO_STANDBY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + /* or from some other armed state like SEATBELT or MANUAL */ + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_MISSION) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_RETURN + && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } + break; + + // XXX we might be missing something that triggers a transition from RTL to LAND + + case ARMING_STATE_ARMED_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } } /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) && - ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && - (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + // printf("checking\n"); + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; + + if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) { + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + + } else { + mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed"); + } + stick_off_counter = 0; } else { stick_off_counter++; @@ -1888,7 +2055,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position --> arm */ if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); stick_on_counter = 0; } else { @@ -1897,49 +2064,24 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check manual override switch - switch to manual or auto mode */ - if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { - /* enable manual override */ - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - - } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - // /* check auto mode switch for correct mode */ - // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - // /* enable guided mode */ - // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - - // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { - // XXX hardcode to auto for now - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - - // } - - } else { - /* center stick position, set SAS for all vehicle types */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } - - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); - - } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } - current_status.rc_signal_cutting_off = false; current_status.rc_signal_lost = false; current_status.rc_signal_lost_interval = 0; } else { - static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ current_status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + + if (!current_status.rc_signal_cutting_off) { + printf("Reason: not rc_signal_cutting_off\n"); + } else { + printf("last print time: %llu\n", last_print_time); + } + last_print_time = hrt_absolute_time(); } @@ -1971,60 +2113,60 @@ int commander_thread_main(int argc, char *argv[]) if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { - /* decide about attitude control flag, enable in att/pos/vel */ - bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + // /* decide about attitude control flag, enable in att/pos/vel */ + // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - /* decide about rate control flag, enable it always XXX (for now) */ - bool rates_ctrl_enabled = true; + // /* decide about rate control flag, enable it always XXX (for now) */ + // bool rates_ctrl_enabled = true; - /* set up control mode */ - if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - state_changed = true; - } + // /* set up control mode */ + // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; + // state_changed = true; + // } - if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - current_status.flag_control_rates_enabled = rates_ctrl_enabled; - state_changed = true; - } + // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + // current_status.flag_control_rates_enabled = rates_ctrl_enabled; + // state_changed = true; + // } - /* handle the case where offboard control signal was regained */ - if (!current_status.offboard_control_signal_found_once) { - current_status.offboard_control_signal_found_once = true; - /* enable offboard control, disable manual input */ - current_status.flag_control_manual_enabled = false; - current_status.flag_control_offboard_enabled = true; - state_changed = true; - tune_confirm(); + // /* handle the case where offboard control signal was regained */ + // if (!current_status.offboard_control_signal_found_once) { + // current_status.offboard_control_signal_found_once = true; + // /* enable offboard control, disable manual input */ + // current_status.flag_control_manual_enabled = false; + // current_status.flag_control_offboard_enabled = true; + // state_changed = true; + // tune_positive(); - mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - } else { - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - state_changed = true; - tune_confirm(); - } - } + // } else { + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); + // state_changed = true; + // tune_positive(); + // } + // } current_status.offboard_control_signal_weak = false; current_status.offboard_control_signal_lost = false; current_status.offboard_control_signal_lost_interval = 0; + // XXX check if this is correct /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_system_armed) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - /* switch to stabilized mode = takeoff */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + if (sp_offboard.armed && !safety.armed) { + + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); + + } else if (!sp_offboard.armed && safety.armed) { - } else if (!sp_offboard.armed && current_status.flag_system_armed) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); } } else { - static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { @@ -2040,7 +2182,7 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.offboard_control_signal_lost_interval > 100000) { current_status.offboard_control_signal_lost = true; current_status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_confirm(); + tune_positive(); /* kill motors after timeout */ if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { @@ -2056,25 +2198,28 @@ int commander_thread_main(int argc, char *argv[]) current_status.timestamp = hrt_absolute_time(); + // XXX this is missing /* If full run came back clean, transition to standby */ - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - current_status.flag_preflight_gyro_calibration == false && - current_status.flag_preflight_mag_calibration == false && - current_status.flag_preflight_accel_calibration == false) { - /* All ok, no calibration going on, go to standby */ - do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - } + // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && + // current_status.flag_preflight_gyro_calibration == false && + // current_status.flag_preflight_mag_calibration == false && + // current_status.flag_preflight_accel_calibration == false) { + // /* All ok, no calibration going on, go to standby */ + // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + // } /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - publish_armed_status(¤t_status); - orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); state_changed = false; } + + /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; - + /* XXX use this voltage_previous */ fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); @@ -2083,6 +2228,7 @@ int commander_thread_main(int argc, char *argv[]) /* wait for threads to complete */ // pthread_join(command_handling_thread, NULL); pthread_join(subsystem_info_thread, NULL); + pthread_join(commander_low_prio_thread, NULL); /* close fds */ led_deinit(); @@ -2093,10 +2239,93 @@ int commander_thread_main(int argc, char *argv[]) close(sensor_sub); close(cmd_sub); - warnx("exiting..\n"); + warnx("exiting"); fflush(stdout); thread_running = false; return 0; } + + +void *commander_low_prio_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander low prio", getpid()); + + while (!thread_should_exit) { + + switch (low_prio_task) { + + case LOW_PRIO_TASK_PARAM_LOAD: + + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_PARAM_SAVE: + + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_GYRO_CALIBRATION: + + do_gyro_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_MAG_CALIBRATION: + + do_mag_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + + // do_baro_calibration(); + + case LOW_PRIO_TASK_RC_CALIBRATION: + + // do_rc_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ACCEL_CALIBRATION: + + do_accel_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; + } + + } + + return 0; +} diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index f42caad57..87aad6270 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -40,32 +40,22 @@ #include <stdio.h> #include <unistd.h> +#include <stdint.h> +#include <stdbool.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_control_mode.h> #include <systemlib/systemlib.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> #include <drivers/drv_hrt.h> #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" #include "commander.h" -static const char *system_state_txt[] = { - "SYSTEM_STATE_PREFLIGHT", - "SYSTEM_STATE_STANDBY", - "SYSTEM_STATE_GROUND_READY", - "SYSTEM_STATE_MANUAL", - "SYSTEM_STATE_STABILIZED", - "SYSTEM_STATE_AUTO", - "SYSTEM_STATE_MISSION_ABORT", - "SYSTEM_STATE_EMCY_LANDING", - "SYSTEM_STATE_EMCY_CUTOFF", - "SYSTEM_STATE_GROUND_ERROR", - "SYSTEM_STATE_REBOOT", - -}; - bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || @@ -80,234 +70,557 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } -/** - * Transition from one state to another - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) -{ - int invalid_state = false; - int ret = ERROR; - - commander_state_machine_t old_state = current_status->state_machine; - - switch (new_state) { - case SYSTEM_STATE_MISSION_ABORT: { - /* Indoor or outdoor */ - // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); - - // } else { - // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); - // } - } - break; +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { - case SYSTEM_STATE_EMCY_LANDING: - /* Tell the controller to land */ - /* set system flags according to state */ - current_status->flag_system_armed = true; - - warnx("EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - /* Tell the controller to cutoff the motors (thrust = 0) */ + int ret = ERROR; - /* set system flags according to state */ - current_status->flag_system_armed = false; + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state == current_state->arming_state) { + ret = OK; + } else { - warnx("EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); - break; + switch (new_arming_state) { + case ARMING_STATE_INIT: - case SYSTEM_STATE_GROUND_ERROR: + /* allow going back from INIT for calibration */ + if (current_state->arming_state == ARMING_STATE_STANDBY) { + ret = OK; + safety->armed = false; + safety->ready_to_arm = false; + } + break; + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (current_state->condition_system_sensors_initialized) { + ret = OK; + safety->armed = false; + safety->ready_to_arm = true; + } else { + mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); + } + } + break; + case ARMING_STATE_ARMED: - /* set system flags according to state */ + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - /* prevent actuators from arming */ - current_status->flag_system_armed = false; + /* XXX conditions for arming? */ + ret = OK; + safety->armed = true; + } + break; + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_ARMED) { + + /* XXX conditions for an error state? */ + ret = OK; + safety->armed = true; + } + break; + case ARMING_STATE_STANDBY_ERROR: + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = OK; + safety->armed = false; + safety->ready_to_arm = false; + } + break; + case ARMING_STATE_REBOOT: - warnx("GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); - break; + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { - case SYSTEM_STATE_PREFLIGHT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); + ret = OK; + safety->armed = false; + safety->ready_to_arm = false; - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); - } + } + break; + case ARMING_STATE_IN_AIR_RESTORE: - break; - - case SYSTEM_STATE_REBOOT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - invalid_state = false; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); + /* XXX implement */ + break; + default: + break; } - break; + if (ret == OK) { + current_state->arming_state = new_arming_state; + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - case SYSTEM_STATE_STANDBY: - /* set system flags according to state */ - - /* standby enforces disarmed */ - current_status->flag_system_armed = false; - - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - break; - - case SYSTEM_STATE_GROUND_READY: + safety->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_safety), safety_pub, safety); + } + } - /* set system flags according to state */ + return ret; +} - /* ground ready has motors / actuators armed */ - current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); - break; - case SYSTEM_STATE_AUTO: +/* + * This functions does not evaluate any input flags but only checks if the transitions + * are valid. + */ +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { - /* set system flags according to state */ + int ret = ERROR; - /* auto is airborne and in auto mode, motors armed */ - current_status->flag_system_armed = true; + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == current_state->navigation_state) { + ret = OK; + } else { - mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); - break; + switch (new_navigation_state) { + case NAVIGATION_STATE_INIT: - case SYSTEM_STATE_STABILIZED: + /* transitions back to INIT are possible for calibration */ + if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { - /* set system flags according to state */ - current_status->flag_system_armed = true; + ret = OK; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_MANUAL_STANDBY: + + /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to be disarmed first */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; + } + } + break; + + case NAVIGATION_STATE_MANUAL: + + /* need to be armed first */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; + } + break; + + case NAVIGATION_STATE_SEATBELT_STANDBY: + + /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + + /* need to be disarmed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_SEATBELT_DESCENT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_STANDBY: + + /* transitions from INIT or from other STANDBY modes or from AUTO READY */ + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + /* need to be disarmed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + tune_negative(); + } else if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + /* transitions from AUTO_STANDBY or AUTO_LAND */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + + // XXX flag test needed? + + /* need to be armed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; - mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); - break; + case NAVIGATION_STATE_AUTO_TAKEOFF: - case SYSTEM_STATE_MANUAL: + /* only transitions from AUTO_READY */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - /* set system flags according to state */ - current_status->flag_system_armed = true; + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a mission ready */ + if (!current_state-> condition_auto_mission_available) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_AUTO_LAND: + /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + + /* need to have a position and home lock */ + if (!current_state->condition_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + tune_negative(); + } else if (!current_state->condition_home_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + } + break; - mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); - break; + default: + break; + } - default: - invalid_state = true; - break; - } + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - if (invalid_state == false || old_state != new_state) { - current_status->state_machine = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - ret = OK; + control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); + } } - if (invalid_state) { - mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); - ret = ERROR; - } + return ret; } -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* publish the new state */ - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - - /* assemble state vector based on flag values */ - if (current_status->flag_control_rates_enabled) { - current_status->onboard_control_sensors_present |= 0x400; - } else { - current_status->onboard_control_sensors_present &= ~0x400; - } +/** +* Transition from one hil state to another +*/ +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); -} + } else { -void publish_armed_status(const struct vehicle_status_s *current_status) -{ - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; + switch (new_state) { - /* XXX allow arming by external components on multicopters only if not yet armed by RC */ - /* XXX allow arming only if core sensors are ok */ - armed.ready_to_arm = true; + case HIL_STATE_OFF: - /* lock down actuators if required, only in HIL */ - armed.lockdown = (current_status->flag_hil_enabled) ? true : false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -} + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; -/* - * Private functions, update the state machine - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - warnx("EMERGENCY HANDLER\n"); - /* Depending on the current state go to one of the error states */ + case HIL_STATE_ON: - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; - // DO NOT abort mission - //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); + default: + warnx("Unknown hil state"); + break; + } + } + if (valid_transition) { + current_status->hil_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + ret = OK; } else { - warnx("Unknown system state: #%d\n", current_status->state_machine); + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); } + + return ret; } -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors + + +void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself - state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); + /* publish the new state */ + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); - } else { - //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); - } + /* assemble state vector based on flag values */ +// if (current_status->flag_control_rates_enabled) { +// current_status->onboard_control_sensors_present |= 0x400; +// +// } else { +// current_status->onboard_control_sensors_present &= ~0x400; +// } + +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); } +// void publish_armed_status(const struct vehicle_status_s *current_status) +// { +// struct actuator_armed_s armed; +// armed.armed = current_status->flag_system_armed; +// +// /* XXX allow arming by external components on multicopters only if not yet armed by RC */ +// /* XXX allow arming only if core sensors are ok */ +// armed.ready_to_arm = true; +// +// /* lock down actuators if required, only in HIL */ +// armed.lockdown = (current_status->flag_hil_enabled) ? true : false; +// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); +// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +// } // /* @@ -420,376 +733,75 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st // } -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //TODO state machine change (recovering) -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_GPS: -// //TODO state machine change -// break; - -// default: -// break; -// } - - -// } - - -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); -// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_GPS: -// // //TODO: remove this block -// // break; -// // /////////////////// -// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); - -// // printf("previosly_healthy = %u\n", previosly_healthy); -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency(status_pub, current_status); - -// break; - -// default: -// break; -// } - -// } - - -/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ - - -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { - state_machine_emergency(status_pub, current_status, mavlink_fd); - } -} - -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - - /* reject arming if safety status is wrong */ - if (current_status->flag_safety_present) { - - /* - * The operator should not touch the vehicle if - * its armed, so we don't allow arming in the - * first place - */ - if (is_rotary_wing(current_status)) { - - /* safety is in safe position, disallow arming */ - if (current_status->flag_safety_safe) { - mavlink_log_critical(mavlink_fd, "DISENGAGE SAFETY BEFORE ARMING!"); - - /* play warning tune */ - tune_error(); - - /* abort */ - return; - } - - } - } - - printf("[cmd] arming\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - } -} - -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[cmd] going standby\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - - } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] MISSION ABORT!\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - - current_status->flag_control_manual_enabled = true; - - /* set behaviour based on airframe */ - if (is_rotary_wing(current_status)) { - - /* assuming a rotary wing, set to SAS */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - - } else { - - /* assuming a fixed wing, set to direct pass-through */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - current_status->flag_control_attitude_enabled = false; - current_status->flag_control_rates_enabled = false; - } - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] manual mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - } -} - -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - int old_mode = current_status->flight_mode; - int old_manual_control_mode = current_status->manual_control_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - current_status->flag_control_manual_enabled = true; - - if (old_mode != current_status->flight_mode || - old_manual_control_mode != current_status->manual_control_mode) { - printf("[cmd] att stabilized mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - state_machine_publish(status_pub, current_status, mavlink_fd); - } - - } -} - -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); - tune_error(); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] position guided mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - } -} - -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[cmd] auto mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - } -} - - -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -{ - uint8_t ret = 1; - - /* Switch on HIL if in standby and not already in HIL mode */ - if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && - current_status->flag_system_armed) { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") - - } else { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") - } - } - - /* switch manual / auto */ - if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { - update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); - } - - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only arm in standby state */ - // XXX REMOVE - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - ret = OK; - printf("[cmd] arming due to command request\n"); - } - } - - /* vehicle is armed, mode requests disarming */ - if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only disarm in ground ready */ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - printf("[cmd] disarming due to command request\n"); - } - } - - /* NEVER actually switch off HIL without reboot */ - if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); - mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); - ret = ERROR; - } - - return ret; -} - -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations -{ - commander_state_machine_t current_system_state = current_status->state_machine; - - uint8_t ret = 1; - - switch (custom_mode) { - case SYSTEM_STATE_GROUND_READY: - break; - - case SYSTEM_STATE_STANDBY: - break; - - case SYSTEM_STATE_REBOOT: - printf("try to reboot\n"); - - if (current_system_state == SYSTEM_STATE_STANDBY - || current_system_state == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - printf("system will reboot\n"); - mavlink_log_critical(mavlink_fd, "Rebooting.."); - usleep(200000); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); - ret = 0; - } - - break; - - case SYSTEM_STATE_AUTO: - printf("try to switch to auto/takeoff\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - printf("state: auto\n"); - ret = 0; - } - - break; - - case SYSTEM_STATE_MANUAL: - printf("try to switch to manual\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - printf("state: manual\n"); - ret = 0; - } - - break; - - default: - break; - } - - return ret; -} - +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int ret = 1; +// +//// /* Switch on HIL if in standby and not already in HIL mode */ +//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) +//// && !current_status->flag_hil_enabled) { +//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { +//// /* Enable HIL on request */ +//// current_status->flag_hil_enabled = true; +//// ret = OK; +//// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); +//// +//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && +//// current_status->flag_fmu_armed) { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") +//// +//// } else { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") +//// } +//// } +// +// /* switch manual / auto */ +// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { +// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { +// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { +// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { +// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); +// } +// +// /* vehicle is disarmed, mode requests arming */ +// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only arm in standby state */ +// // XXX REMOVE +// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); +// ret = OK; +// printf("[cmd] arming due to command request\n"); +// } +// } +// +// /* vehicle is armed, mode requests disarming */ +// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only disarm in ground ready */ +// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); +// ret = OK; +// printf("[cmd] disarming due to command request\n"); +// } +// } +// +// /* NEVER actually switch off HIL without reboot */ +// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { +// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); +// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); +// ret = ERROR; +// } +// +// return ret; +//} diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 95b48d326..b015c4efe 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -46,168 +46,22 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_safety.h> +#include <uORB/topics/vehicle_control_mode.h> -bool is_multirotor(const struct vehicle_status_s *current_status); - -bool is_rotary_wing(const struct vehicle_status_s *current_status); - -/** - * Switch to new state with no checking. - * - * do_state_update: this is the functions that all other functions have to call in order to update the state. - * the function does not question the state change, this must be done before - * The function performs actions that are connected with the new state (buzzer, reboot, ...) - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - * - * @return 0 (macro OK) or 1 on error (macro ERROR) - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); -/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - - -/** - * Handle state machine if got position fix - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if position fix lost - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to arm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to disarm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is manual - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is stabilized - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +bool is_multirotor(const struct vehicle_status_s *current_status); -/** - * Handle state machine if mode switch is guided - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +bool is_rotary_wing(const struct vehicle_status_s *current_status); -/** - * Handle state machine if mode switch is auto - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); -/** - * Publish current state information - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); -/* - * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). - * If the request is obeyed the functions return 0 - * - */ - -/** - * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); - -/** - * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); - -/** - * Always switches to critical mode under any circumstances. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Switches to emergency if required. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish the armed state depending on the current system state - * - * @param current_status the current system status - */ -void publish_armed_status(const struct vehicle_status_s *current_status); - - +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp index 7be38015c..8b0ea2fac 100644 --- a/src/modules/controllib/fixedwing.cpp +++ b/src/modules/controllib/fixedwing.cpp @@ -39,6 +39,7 @@ #include "fixedwing.hpp" + namespace control { @@ -210,8 +211,9 @@ void BlockMultiModeBacksideAutopilot::update() for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) _actuators.control[i] = 0.0f; +#warning this if is incomplete, should be based on flags // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); } @@ -220,9 +222,10 @@ void BlockMultiModeBacksideAutopilot::update() // once the system switches from manual or auto to stabilized // the setpoint should update to loitering around this position +#warning should be base on flags // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || + _status.navigation_state == NAVIGATION_STATE_SEATBELT) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); @@ -280,15 +283,16 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_THR] : _manual.throttle; } - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; + } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { +#warning fix here too +// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +// _actuators.control[CH_AIL] = _manual.roll; +// _actuators.control[CH_ELV] = _manual.pitch; +// _actuators.control[CH_RDR] = _manual.yaw; +// _actuators.control[CH_THR] = _manual.throttle; +// +// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between @@ -344,7 +348,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? _actuators.control[CH_THR] : _manual.throttle; } - } +#warning fix this +// } // body rates controller, disabled for now else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index 769b8b0a8..7009356b7 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller - pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller initialized = true; } @@ -137,12 +137,12 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim); - pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); + pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0); + pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0); } /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL); /* Pitch (P) */ @@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* set pitch plus feedforward roll compensation */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); + att->pitch, 0, 0, NULL, NULL, NULL); /* Yaw (from coordinated turn constraint or lateral force) */ rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c index 58477632b..758c97d78 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c @@ -186,87 +186,88 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* control */ - if (vstatus.state_machine == SYSTEM_STATE_AUTO || - vstatus.state_machine == SYSTEM_STATE_STABILIZED) { - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost) { - - /* put plane into loiter */ - att_sp.roll_body = 0.3f; - att_sp.pitch_body = 0.0f; - - /* limit throttle to 60 % of last value if sane */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.0f) && - (manual_sp.throttle <= 1.0f)) { - att_sp.thrust = 0.6f * manual_sp.throttle; - - } else { - att_sp.thrust = 0.0f; - } - - att_sp.yaw_body = 0; - - // XXX disable yaw control, loiter - - } else { - - att_sp.roll_body = manual_sp.roll; - att_sp.pitch_body = manual_sp.pitch; - att_sp.yaw_body = 0; - att_sp.thrust = manual_sp.throttle; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* pass through flaps */ - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - - } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; - - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - } - } +#warning fix this +// if (vstatus.state_machine == SYSTEM_STATE_AUTO || +// vstatus.state_machine == SYSTEM_STATE_STABILIZED) { +// /* attitude control */ +// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); +// +// /* angular rate control */ +// fixedwing_att_control_rates(&rates_sp, gyro, &actuators); +// +// /* pass through throttle */ +// actuators.control[3] = att_sp.thrust; +// +// /* set flaps to zero */ +// actuators.control[4] = 0.0f; +// +// } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { +// if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { +// +// /* if the RC signal is lost, try to stay level and go slowly back down to ground */ +// if (vstatus.rc_signal_lost) { +// +// /* put plane into loiter */ +// att_sp.roll_body = 0.3f; +// att_sp.pitch_body = 0.0f; +// +// /* limit throttle to 60 % of last value if sane */ +// if (isfinite(manual_sp.throttle) && +// (manual_sp.throttle >= 0.0f) && +// (manual_sp.throttle <= 1.0f)) { +// att_sp.thrust = 0.6f * manual_sp.throttle; +// +// } else { +// att_sp.thrust = 0.0f; +// } +// +// att_sp.yaw_body = 0; +// +// // XXX disable yaw control, loiter +// +// } else { +// +// att_sp.roll_body = manual_sp.roll; +// att_sp.pitch_body = manual_sp.pitch; +// att_sp.yaw_body = 0; +// att_sp.thrust = manual_sp.throttle; +// } +// +// att_sp.timestamp = hrt_absolute_time(); +// +// /* attitude control */ +// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); +// +// /* angular rate control */ +// fixedwing_att_control_rates(&rates_sp, gyro, &actuators); +// +// /* pass through throttle */ +// actuators.control[3] = att_sp.thrust; +// +// /* pass through flaps */ +// if (isfinite(manual_sp.flaps)) { +// actuators.control[4] = manual_sp.flaps; +// +// } else { +// actuators.control[4] = 0.0f; +// } +// +// } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +// /* directly pass through values */ +// actuators.control[0] = manual_sp.roll; +// /* positive pitch means negative actuator -> pull up */ +// actuators.control[1] = manual_sp.pitch; +// actuators.control[2] = manual_sp.yaw; +// actuators.control[3] = manual_sp.throttle; +// +// if (isfinite(manual_sp.flaps)) { +// actuators.control[4] = manual_sp.flaps; +// +// } else { +// actuators.control[4] = 0.0f; +// } +// } +// } /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 4eccc118c..991d5ec5d 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher initialized = true; } @@ -189,18 +189,18 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); + pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0); + pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0); } /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL); counter++; 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 71c78f5b8..9c19c6786 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) parameters_init(&h); parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit - pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, 0.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, 0.0f, PID_MODE_DERIVATIV_NONE); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, 0.0f, PID_MODE_DERIVATIV_NONE); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD_F, 0.0f, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value /* error and performance monitoring */ perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); @@ -268,10 +268,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit - pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value + pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f, 0.0f); //arbitrary high limit + pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim, 0.0f); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim, 0.0f); + pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F, 0.0f); //TODO: remove hardcoded value } /* only run controller if attitude changed */ @@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) // XXX what is xtrack_err.past_end? if (distance_res == OK /*&& !xtrack_err.past_end*/) { - float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance float psi_c = psi_track + delta_psi_c; float psi_e = psi_c - att.yaw; @@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) } /* calculate roll setpoint, do this artificially around zero */ - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL); float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following float psi_rate_c = delta_psi_rate_c + psi_rate_track; @@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL); if (verbose) { printf("psi_rate_c %.4f ", (double)psi_rate_c); @@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) if (pos_updated) { //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL); } diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 43cbe74c7..618095d0b 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,6 +51,7 @@ #include <systemlib/err.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_safety.h> #include <poll.h> #include <drivers/drv_gpio.h> @@ -60,6 +61,8 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; + struct actuator_safety_s safety; + int actuator_safety_sub; bool led_state; int counter; }; @@ -168,10 +171,15 @@ void gpio_led_cycle(FAR void *arg) if (status_updated) orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + orb_check(priv->vehicle_status_sub, &status_updated); + + if (status_updated) + orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety); + /* select pattern for current status */ int pattern = 0; - if (priv->status.flag_system_armed) { + if (priv->safety.armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -180,11 +188,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { + if (priv->safety.ready_to_arm) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && - priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + } else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) } else { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index de78cd139..15ba8860d 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -195,95 +195,88 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* reset MAVLink mode bitfield */ *mavlink_mode = 0; - /* set mode flags independent of system state */ + /** + * Set mode flags + **/ /* HIL */ if (v_status.flag_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - /* manual input */ - if (v_status.flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - } - - /* attitude or rate control */ - if (v_status.flag_control_attitude_enabled || - v_status.flag_control_rates_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - /* vector control */ - if (v_status.flag_control_velocity_enabled || - v_status.flag_control_position_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - /* autonomous mode */ - if (v_status.state_machine == SYSTEM_STATE_AUTO) { + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ - if (armed.armed) { + if (v_status.arming_state == ARMING_STATE_ARMED + || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - switch (v_status.state_machine) { - case SYSTEM_STATE_PREFLIGHT: - if (v_status.flag_preflight_gyro_calibration || - v_status.flag_preflight_mag_calibration || - v_status.flag_preflight_accel_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; + if (v_status.navigation_state == NAVIGATION_STATE_MANUAL + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { - } else { - *mavlink_state = MAV_STATE_UNINIT; - } + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } - break; + if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) { - case SYSTEM_STATE_STANDBY: - *mavlink_state = MAV_STATE_STANDBY; - break; + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - break; + /** + * Set mavlink state + **/ - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - break; + /* set calibration state */ + if (v_status.flag_preflight_calibration) { - case SYSTEM_STATE_AUTO: - *mavlink_state = MAV_STATE_ACTIVE; - break; + *mavlink_state = MAV_STATE_CALIBRATING; - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; + } else if (v_status.flag_system_emergency) { - case SYSTEM_STATE_EMCY_LANDING: *mavlink_state = MAV_STATE_EMERGENCY; - break; - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; + // XXX difference between active and armed? is AUTO_READY active? + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; + *mavlink_state = MAV_STATE_ACTIVE; - case SYSTEM_STATE_REBOOT: - *mavlink_state = MAV_STATE_POWEROFF; - break; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { + + *mavlink_state = MAV_STATE_STANDBY; + + } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { + + *mavlink_state = MAV_STATE_UNINIT; + } else { + + warnx("Unknown mavlink state"); + *mavlink_state = MAV_STATE_CRITICAL; } } @@ -579,6 +572,7 @@ int mavlink_thread_main(int argc, char *argv[]) default: usage(); + break; } } @@ -689,7 +683,7 @@ int mavlink_thread_main(int argc, char *argv[]) get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* switch HIL mode if required */ set_hil_on_off(v_status.flag_hil_enabled); diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 9b2d984f0..56cbdb26b 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -71,7 +71,7 @@ struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; -struct actuator_armed_s armed; +struct actuator_safety_s safety; struct mavlink_subscriptions mavlink_subs; @@ -109,7 +109,7 @@ static void l_global_position_setpoint(const struct listener *l); static void l_local_position_setpoint(const struct listener *l); static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); -static void l_actuator_armed(const struct listener *l); +static void l_actuator_safety(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); @@ -133,7 +133,7 @@ static const struct listener listeners[] = { {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_armed, &mavlink_subs.armed_sub, 0}, + {l_actuator_safety, &mavlink_subs.safety_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, @@ -269,7 +269,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(actuator_safety), mavlink_subs.safety_sub, &safety); /* enable or disable HIL */ set_hil_on_off(v_status.flag_hil_enabled); @@ -284,7 +284,7 @@ l_vehicle_status(const struct listener *l) mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, - v_status.state_machine, + v_status.navigation_state, mavlink_state); } @@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled && armed.armed) { + if (mavlink_hil_enabled && safety.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l) } void -l_actuator_armed(const struct listener *l) +l_actuator_safety(const struct listener *l) { - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); } void @@ -745,8 +745,8 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ + mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index d61cd43dc..221957d46 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -54,10 +54,12 @@ #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_safety.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_rc_input.h> @@ -72,7 +74,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 5a2685560..dbea2be08 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -274,83 +274,89 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ *mavlink_mode = 0; /* set mode flags independent of system state */ - if (v_status->flag_control_manual_enabled) { + if (control_mode->flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status->flag_hil_enabled) { + if (safety->hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* set arming state */ - if (armed->armed) { + if (safety->armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - switch (v_status->state_machine) { - case SYSTEM_STATE_PREFLIGHT: - if (v_status->flag_preflight_gyro_calibration || - v_status->flag_preflight_mag_calibration || - v_status->flag_preflight_accel_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else { - *mavlink_state = MAV_STATE_UNINIT; - } - break; - - case SYSTEM_STATE_STANDBY: - *mavlink_state = MAV_STATE_STANDBY; - break; - - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - break; - - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - break; - - case SYSTEM_STATE_AUTO: - *mavlink_state = MAV_STATE_ACTIVE; + if (control_mode->flag_control_velocity_enabled) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - break; - - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_LANDING: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_REBOOT: - *mavlink_state = MAV_STATE_POWEROFF; - break; + } else { + *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; } +// switch (v_status->state_machine) { +// case SYSTEM_STATE_PREFLIGHT: +// if (v_status->flag_preflight_gyro_calibration || +// v_status->flag_preflight_mag_calibration || +// v_status->flag_preflight_accel_calibration) { +// *mavlink_state = MAV_STATE_CALIBRATING; +// } else { +// *mavlink_state = MAV_STATE_UNINIT; +// } +// break; +// +// case SYSTEM_STATE_STANDBY: +// *mavlink_state = MAV_STATE_STANDBY; +// break; +// +// case SYSTEM_STATE_GROUND_READY: +// *mavlink_state = MAV_STATE_ACTIVE; +// break; +// +// case SYSTEM_STATE_MANUAL: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; +// break; +// +// case SYSTEM_STATE_STABILIZED: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; +// break; +// +// case SYSTEM_STATE_AUTO: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; +// break; +// +// case SYSTEM_STATE_MISSION_ABORT: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_LANDING: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_CUTOFF: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_GROUND_ERROR: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_REBOOT: +// *mavlink_state = MAV_STATE_POWEROFF; +// break; +// } + } /** @@ -362,8 +368,10 @@ int mavlink_thread_main(int argc, char *argv[]) char *device_name = "/dev/ttyS1"; baudrate = 57600; + /* XXX this is never written? */ struct vehicle_status_s v_status; - struct actuator_armed_s armed; + struct vehicle_control_mode_s control_mode; + struct actuator_safety_s safety; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -431,10 +439,10 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 0acbea675..2d406fb9f 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -328,4 +328,4 @@ receive_start(int uart) pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); return thread; -}
\ No newline at end of file +} diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f18f56243..f01f09e12 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -52,9 +52,11 @@ #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_safety.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_rc_input.h> @@ -69,7 +71,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 38a4db372..c6a2e52bf 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,6 @@ extern volatile bool thread_should_exit; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +extern void +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c..c3b2f5d2a 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -57,12 +57,14 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <drivers/drv_gyro.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/actuator_safety.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_control_debug.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/parameter_update.h> @@ -83,14 +85,17 @@ static int mc_task; static bool motor_test_mode = false; static orb_advert_t actuator_pub; +static orb_advert_t control_debug_pub; -static struct vehicle_status_s state; static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); + struct actuator_safety_s safety; + memset(&safety, 0, sizeof(safety)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -107,12 +112,16 @@ mc_thread_main(int argc, char *argv[]) struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); + struct vehicle_control_debug_s control_debug; + memset(&control_debug, 0, sizeof(control_debug)); + /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int param_sub = orb_subscribe(ORB_ID(parameter_update)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -133,6 +142,8 @@ mc_thread_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } + + control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); @@ -150,7 +161,9 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool flag_system_armed = false; + bool flag_armed = false; + bool flag_control_position_enabled = false; + bool flag_control_velocity_enabled = false; /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; @@ -162,7 +175,6 @@ mc_thread_main(int argc, char *argv[]) param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); float failsafe_throttle = 0.0f; - while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -175,7 +187,6 @@ mc_thread_main(int argc, char *argv[]) } else if (ret == 0) { /* no return value, ignore */ } else { - /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ @@ -193,10 +204,16 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of system state */ bool updated; - orb_check(state_sub, &updated); + orb_check(control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); + } + + orb_check(safety_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); } /* get a local copy of manual setpoint */ @@ -215,9 +232,8 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - /** STEP 1: Define which input is the dominating control input */ - if (state.flag_control_offboard_enabled) { + if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { rates_sp.roll = offboard_sp.p1; @@ -240,20 +256,20 @@ mc_thread_main(int argc, char *argv[]) } - } else if (state.flag_control_manual_enabled) { - - if (state.flag_control_attitude_enabled) { - + } else if (control_mode.flag_control_manual_enabled) { + if (control_mode.flag_control_attitude_enabled) { /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { + if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || + control_mode.flag_control_manual_enabled != flag_control_manual_enabled || + safety.armed != flag_armed) { att_sp.yaw_body = att.yaw; } static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ +/* XXX fix this */ +#if 0 if (state.rc_signal_lost) { /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); @@ -285,33 +301,35 @@ mc_thread_main(int argc, char *argv[]) rc_loss_first_time = false; } else { +#endif rc_loss_first_time = true; att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { + if (!flag_control_attitude_enabled && safety.armed) { att_sp.yaw_body = att.yaw; } /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - - } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ - +#warning fix this +// if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || +// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +// +// if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { +// rates_sp.yaw = manual.yaw; +// control_yaw_position = false; +// +// } else { +// /* +// * This mode SHOULD be the default mode, which is: +// * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS +// * +// * However, we fall back to this setting for all other (nonsense) +// * settings as well. +// */ +// /* only move setpoint if manual input is != 0 */ if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { rates_sp.yaw = manual.yaw; @@ -326,16 +344,19 @@ mc_thread_main(int argc, char *argv[]) control_yaw_position = true; } - } - } +// } +// } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); +#if 0 } +#endif /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + if (motor_test_mode) { printf("testmode"); att_sp.roll_body = 0.0f; @@ -348,23 +369,24 @@ mc_thread_main(int argc, char *argv[]) } } else { +#warning fix this /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; - - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } +// if (state.flag_control_rates_enabled && +// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { +// rates_sp.roll = manual.roll; +// +// rates_sp.pitch = manual.pitch; +// rates_sp.yaw = manual.yaw; +// rates_sp.thrust = manual.throttle; +// rates_sp.timestamp = hrt_absolute_time(); +// } } } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - if (state.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); + if (control_mode.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, &control_debug_pub, &control_debug); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } @@ -387,13 +409,17 @@ mc_thread_main(int argc, char *argv[]) gyro[1] = att.pitchspeed; gyro[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, gyro, &actuators); + multirotor_control_rates(&rates_sp, gyro, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* update state */ - flag_control_attitude_enabled = state.flag_control_attitude_enabled; - flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_system_armed = state.flag_system_armed; + orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); + + /* update control_mode */ + flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; + flag_control_manual_enabled = control_mode.flag_control_manual_enabled; + flag_control_position_enabled = control_mode.flag_control_position_enabled; + flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled; + flag_armed = safety.armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ @@ -410,7 +436,7 @@ mc_thread_main(int argc, char *argv[]) close(att_sub); - close(state_sub); + close(control_mode_sub); close(manual_sub); close(actuator_pub); close(att_sp_pub); diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 76dbb36d3..88cc65148 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -57,50 +57,29 @@ PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); - -//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); struct mc_att_control_params { float yaw_p; float yaw_i; float yaw_d; - //float yaw_awu; - //float yaw_lim; float att_p; float att_i; float att_d; - //float att_awu; - //float att_lim; - - //float att_xoff; - //float att_yoff; }; struct mc_att_control_param_handles { param_t yaw_p; param_t yaw_i; param_t yaw_d; - //param_t yaw_awu; - //param_t yaw_lim; param_t att_p; param_t att_i; param_t att_d; - //param_t att_awu; - //param_t att_lim; - - //param_t att_xoff; - //param_t att_yoff; }; /** @@ -122,17 +101,10 @@ static int parameters_init(struct mc_att_control_param_handles *h) h->yaw_p = param_find("MC_YAWPOS_P"); h->yaw_i = param_find("MC_YAWPOS_I"); h->yaw_d = param_find("MC_YAWPOS_D"); - //h->yaw_awu = param_find("MC_YAWPOS_AWU"); - //h->yaw_lim = param_find("MC_YAWPOS_LIM"); h->att_p = param_find("MC_ATT_P"); h->att_i = param_find("MC_ATT_I"); h->att_d = param_find("MC_ATT_D"); - //h->att_awu = param_find("MC_ATT_AWU"); - //h->att_lim = param_find("MC_ATT_LIM"); - - //h->att_xoff = param_find("MC_ATT_XOFF"); - //h->att_yoff = param_find("MC_ATT_YOFF"); return OK; } @@ -142,23 +114,17 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc param_get(h->yaw_p, &(p->yaw_p)); param_get(h->yaw_i, &(p->yaw_i)); param_get(h->yaw_d, &(p->yaw_d)); - //param_get(h->yaw_awu, &(p->yaw_awu)); - //param_get(h->yaw_lim, &(p->yaw_lim)); param_get(h->att_p, &(p->att_p)); param_get(h->att_i, &(p->att_i)); param_get(h->att_d, &(p->att_d)); - //param_get(h->att_awu, &(p->att_awu)); - //param_get(h->att_lim, &(p->att_lim)); - - //param_get(h->att_xoff, &(p->att_xoff)); - //param_get(h->att_yoff, &(p->att_yoff)); return OK; } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -170,8 +136,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s last_input = att_sp->timestamp; } - static int sensor_delay; - sensor_delay = hrt_absolute_time() - att->timestamp; +// static int sensor_delay; +// sensor_delay = hrt_absolute_time() - att->timestamp; static int motor_skip_counter = 0; @@ -190,10 +156,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -204,8 +168,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); } /* reset integral if on ground */ @@ -219,11 +183,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); + att->pitch, att->pitchspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); + att->roll, att->rollspeed, deltaT, &control_debug->roll_p, &control_debug->roll_i, &control_debug->roll_d); + + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); if (control_yaw_position) { /* control yaw rate */ diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h index 2cf83e443..4e70a5103 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -50,8 +50,11 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_control_debug.h> + void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index deba1ac03..a39d6f13d 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -54,6 +54,9 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> +#include <uORB/uORB.h> + + PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); @@ -148,15 +151,14 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], struct actuator_controls_s *actuators, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { - static float roll_control_last = 0; - static float pitch_control_last = 0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; static uint64_t last_input = 0; - float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; +// float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; if (last_input != rate_sp->timestamp) { last_input = rate_sp->timestamp; @@ -166,49 +168,60 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static int motor_skip_counter = 0; + static PID_t pitch_rate_controller; + static PID_t roll_rate_controller; + static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; + float pitch_control_last = 0.0f; + float roll_control_last = 0.0f; + static bool initialized = false; + /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { parameters_init(&h); parameters_update(&h, &p); initialized = true; + + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + } /* load new parameters with lower rate */ if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - // warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz", - // (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } - /* calculate current control outputs */ - /* control pitch (forward) output */ - float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last); + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , + rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + + /* control roll (left/right) output */ + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , + rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { pitch_control_last = pitch_control; } else { - pitch_control = 0.0f; + pitch_control = pitch_control_last; warnx("rej. NaN ctrl pitch"); } - /* control roll (left/right) output */ - float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last); - /* increase resilience to faulty control inputs */ if (isfinite(roll_control)) { roll_control_last = roll_control; } else { - roll_control = 0.0f; + roll_control = roll_control_last; warnx("rej. NaN ctrl roll"); } diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 03dec317a..465549540 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -49,8 +49,10 @@ #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_control_debug.h> void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], struct actuator_controls_s *actuators, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index b654bdec4..e3ec2fa76 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -59,6 +59,11 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 200000 +/* + * Time that the ESCs need to initialize + */ + #define ESC_INIT_TIME_US 1000000 + /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 #define PITCH 1 @@ -69,6 +74,10 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; +static uint64_t esc_init_time; +static bool esc_init_active = false; +static bool esc_init_done = false; + /* selected control values and count for mixing */ enum mixer_source { MIX_NONE, @@ -166,6 +175,20 @@ mixer_tick(void) float outputs[IO_SERVO_COUNT]; unsigned mixed; + /* after arming, some ESCs need an initalization period, count the time from here */ + if (mixer_servos_armed && !esc_init_done && !esc_init_active) { + esc_init_time = hrt_absolute_time(); + esc_init_active = true; + isr_debug(1, "start counting now"); + } + + /* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */ + if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) { + esc_init_active = false; + esc_init_done = true; + isr_debug(1, "time is up"); + } + /* mix */ mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); @@ -175,8 +198,20 @@ mixer_tick(void) /* save actuator values for FMU readback */ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - /* scale to servo output */ - r_page_servos[i] = (outputs[i] * 600.0f) + 1500; + /* XXX maybe this check for an armed FMU could be achieved a little less messy */ + if (source == MIX_FMU && !(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) { + r_page_servos[i] = r_page_servo_failsafe[i]; + } + /* during ESC initialization, use low PWM */ + else if (esc_init_active) { + r_page_servos[i] = (outputs[i] * 600 + 1500); + + /* afterwards use min and max values */ + } else { + r_page_servos[i] = (outputs[i] + * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); + } } for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) @@ -193,13 +228,15 @@ mixer_tick(void) * here. */ bool should_arm = ( - /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - /* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || - /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) && - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + /* and either FMU is armed */ ( ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && + /* and there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))) || + + /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) ); + if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); @@ -213,6 +250,9 @@ mixer_tick(void) mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> disarmed"); + + esc_init_active = false; + isr_debug(1, "disarming, and init aborted"); } if (mixer_servos_armed) { @@ -345,6 +385,7 @@ mixer_set_failsafe() * Check if a custom failsafe value has been written, * or if the mixer is not ok and bail out. */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) return; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 6c6c7b4e2..b43054197 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -187,6 +187,12 @@ /* PWM failsafe values - zero disables the output */ #define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + + /* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + /** * As-needed mixer data upload. * diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf..3e4027c9b 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -77,6 +77,8 @@ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ +extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ +extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ /* * Register aliases. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 148514455..379cf09e3 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -146,7 +146,8 @@ volatile uint16_t r_page_setup[] = #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ - PX4IO_P_SETUP_ARMING_IO_ARM_OK) + PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \ + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM #define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -184,6 +185,22 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; +/** + * PAGE 106 + * + * minimum PWM values when armed + * + */ +uint16_t r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + +/** + * PAGE 107 + * + * maximum PWM values when armed + * + */ +uint16_t r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -247,6 +264,50 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) + /* set to default */ + r_page_servo_control_min[offset] = 900; + + else if (*values > 1200) + r_page_servo_control_min[offset] = 1200; + else if (*values < 900) + r_page_servo_control_min[offset] = 900; + else + r_page_servo_control_min[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_CONTROL_MAX_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) + /* set to default */ + r_page_servo_control_max[offset] = 2100; + + else if (*values > 2100) + r_page_servo_control_max[offset] = 2100; + else if (*values < 1800) + r_page_servo_control_max[offset] = 1800; + else + r_page_servo_control_max[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: mixer_handle_text(values, num_values * sizeof(*values)); @@ -584,6 +645,12 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + SELECT_PAGE(r_page_servo_control_min); + break; + case PX4IO_PAGE_CONTROL_MAX_PWM: + SELECT_PAGE(r_page_servo_control_max); + break; default: return -1; diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 8aaafaf31..2e1e4fd4d 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -121,7 +121,7 @@ int logbuffer_get_ptr(struct logbuffer_s *lb, void **ptr, bool *is_part) } else { // read pointer is after write pointer, read bytes from read_ptr to end of the buffer n = lb->size - lb->read_ptr; - *is_part = true; + *is_part = lb->write_ptr > 0; } *ptr = &(lb->data[lb->read_ptr]); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a14bd6f80..f967638c3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -60,10 +60,12 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> +#include <uORB/topics/actuator_safety.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> @@ -73,6 +75,7 @@ #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vehicle_control_debug.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/differential_pressure.h> @@ -102,7 +105,7 @@ //#define SDLOG2_DEBUG -static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ @@ -113,35 +116,34 @@ static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; static const char *mountpoint = "/fs/microsd"; -int log_file = -1; -int mavlink_fd = -1; +static int mavlink_fd = -1; struct logbuffer_s lb; /* mutex / condition to synchronize threads */ -pthread_mutex_t logbuffer_mutex; -pthread_cond_t logbuffer_cond; +static pthread_mutex_t logbuffer_mutex; +static pthread_cond_t logbuffer_cond; -char folder_path[64]; +static char folder_path[64]; /* statistics counters */ -unsigned long log_bytes_written = 0; -uint64_t start_time = 0; -unsigned long log_msgs_written = 0; -unsigned long log_msgs_skipped = 0; +static unsigned long log_bytes_written = 0; +static uint64_t start_time = 0; +static unsigned long log_msgs_written = 0; +static unsigned long log_msgs_skipped = 0; /* current state of logging */ -bool logging_enabled = false; +static bool logging_enabled = false; /* enable logging on start (-e option) */ -bool log_on_start = false; +static bool log_on_start = false; /* enable logging when armed (-a option) */ -bool log_when_armed = false; +static bool log_when_armed = false; /* delay = 1 / rate (rate defined by -r option) */ -useconds_t sleep_delay = 0; +static useconds_t sleep_delay = 0; /* helper flag to track system state changes */ -bool flag_system_armed = false; +static bool flag_system_armed = false; -pthread_t logwriter_pthread = 0; +static pthread_t logwriter_pthread = 0; /** * Log buffer writing thread. Open and close file here. @@ -171,17 +173,17 @@ static void sdlog2_status(void); /** * Start logging: create new file and start log writer thread. */ -void sdlog2_start_log(); +static void sdlog2_start_log(void); /** * Stop logging: stop log writer thread and close log file. */ -void sdlog2_stop_log(); +static void sdlog2_stop_log(void); /** * Write a header to log file: list of message formats. */ -void write_formats(int fd); +static void write_formats(int fd); static bool file_exist(const char *filename); @@ -190,17 +192,17 @@ static int file_copy(const char *file_old, const char *file_new); static void handle_command(struct vehicle_command_s *cmd); -static void handle_status(struct vehicle_status_s *cmd); +static void handle_status(struct actuator_safety_s *safety); /** * Create folder for current logging session. Store folder name in 'log_folder'. */ -static int create_logfolder(); +static int create_logfolder(void); /** * Select first free log file name and open it. */ -static int open_logfile(); +static int open_logfile(void); static void sdlog2_usage(const char *reason) @@ -236,11 +238,11 @@ int sdlog2_main(int argc, char *argv[]) exit(0); } - thread_should_exit = false; + main_thread_should_exit = false; deamon_task = task_spawn("sdlog2", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 30, - 2048, + 3000, sdlog2_thread_main, (const char **)argv); exit(0); @@ -251,7 +253,7 @@ int sdlog2_main(int argc, char *argv[]) printf("\tsdlog2 is not started\n"); } - thread_should_exit = true; + main_thread_should_exit = true; exit(0); } @@ -285,22 +287,6 @@ int create_logfolder() if (mkdir_ret == 0) { /* folder does not exist, success */ - - /* copy parser script file */ - // TODO - /* - char mfile_out[100]; - sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber); - int ret = file_copy(mfile_in, mfile_out); - - if (!ret) { - warnx("copied m file to %s", mfile_out); - - } else { - warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out); - } - */ - break; } else if (mkdir_ret == -1) { @@ -346,10 +332,10 @@ int open_logfile() fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC); if (fd == 0) { - errx(1, "opening %s failed.", path_buf); + warn("opening %s failed", path_buf); } - warnx("logging to: %s", path_buf); + warnx("logging to: %s.", path_buf); mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf); return fd; @@ -357,7 +343,7 @@ int open_logfile() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warn("all %d possible files exist already", MAX_NO_LOGFILE); + warnx("all %d possible files exist already.", MAX_NO_LOGFILE); return -1; } @@ -383,8 +369,7 @@ static void *logwriter_thread(void *arg) bool should_wait = false; bool is_part = false; - while (!thread_should_exit && !logwriter_should_exit) { - + while (true) { /* make sure threads are synchronized */ pthread_mutex_lock(&logbuffer_mutex); @@ -394,7 +379,7 @@ static void *logwriter_thread(void *arg) } /* only wait if no data is available to process */ - if (should_wait) { + if (should_wait && !logwriter_should_exit) { /* blocking wait for new data at this line */ pthread_cond_wait(&logbuffer_cond, &logbuffer_mutex); } @@ -402,6 +387,11 @@ static void *logwriter_thread(void *arg) /* only get pointer to thread-safe data, do heavy I/O a few lines down */ int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part); +#ifdef SDLOG2_DEBUG + int rp = logbuf->read_ptr; + int wp = logbuf->write_ptr; +#endif + /* continue */ pthread_mutex_unlock(&logbuffer_mutex); @@ -418,11 +408,11 @@ static void *logwriter_thread(void *arg) should_wait = (n == available) && !is_part; #ifdef SDLOG2_DEBUG - printf("%i wrote: %i of %i, is_part=%i, should_wait=%i", poll_count, n, available, (int)is_part, (int)should_wait); + printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait); #endif if (n < 0) { - thread_should_exit = true; + main_thread_should_exit = true; err(1, "error writing log file"); } @@ -431,19 +421,33 @@ static void *logwriter_thread(void *arg) } } else { + n = 0; +#ifdef SDLOG2_DEBUG + printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit); +#endif + /* exit only with empty buffer */ + if (main_thread_should_exit || logwriter_should_exit) { +#ifdef SDLOG2_DEBUG + printf("break logwriter thread\n"); +#endif + break; + } should_wait = true; } - if (poll_count % 10 == 0) { + if (++poll_count == 10) { fsync(log_file); + poll_count = 0; } - - poll_count++; } fsync(log_file); close(log_file); +#ifdef SDLOG2_DEBUG + printf("logwriter thread exit\n"); +#endif + return OK; } @@ -470,10 +474,9 @@ void sdlog2_start_log() pthread_attr_setstacksize(&receiveloop_attr, 2048); logwriter_should_exit = false; - pthread_t thread; /* start log buffer emptying thread */ - if (0 != pthread_create(&thread, &receiveloop_attr, logwriter_thread, &lb)) { + if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) { errx(1, "error creating logwriter thread"); } @@ -487,16 +490,20 @@ void sdlog2_stop_log() mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; - logwriter_should_exit = true; /* wake up write thread one last time */ pthread_mutex_lock(&logbuffer_mutex); + logwriter_should_exit = true; pthread_cond_signal(&logbuffer_cond); /* unlock, now the writer thread may return */ pthread_mutex_unlock(&logbuffer_mutex); /* wait for write thread to return */ - (void)pthread_join(logwriter_pthread, NULL); + int ret; + if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) { + warnx("error joining logwriter thread: %i", ret); + } + logwriter_pthread = 0; sdlog2_status(); } @@ -517,7 +524,7 @@ void write_formats(int fd) for (i = 0; i < log_formats_num; i++) { log_format_packet.body = log_formats[i]; - write(fd, &log_format_packet, sizeof(log_format_packet)); + log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet)); } fsync(fd); @@ -607,12 +614,10 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "can't allocate log buffer, exiting."); } - /* file descriptors to wait for */ - struct pollfd fds_control[2]; - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 15; + const ssize_t fdsc = 19; + /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -621,12 +626,16 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); + struct actuator_safety_s buf_safety; + memset(&buf_safety, 0, sizeof(buf_safety)); + /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; + struct vehicle_rates_setpoint_s rates_sp; struct actuator_outputs_s act_outputs; struct actuator_controls_s act_controls; struct actuator_controls_effective_s act_controls_effective; @@ -635,6 +644,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; + struct vehicle_control_debug_s control_debug; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -645,9 +655,11 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; + int safety_sub; int sensor_sub; int att_sub; int att_sp_sub; + int rates_sp_sub; int act_outputs_sub; int act_controls_sub; int act_controls_effective_sub; @@ -656,8 +668,10 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int gps_pos_sub; int vicon_pos_sub; + int control_debug_sub; int flow_sub; int rc_sub; + int airspeed_sub; } subs; /* log message buffer: header + body */ @@ -675,8 +689,11 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; + struct log_CTRL_s log_CTRL; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; + struct log_AIRS_s log_AIRS; + struct log_ARSP_s log_ARSP; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -690,6 +707,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- SAFETY --- */ + subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + fds[fdsc_count].fd = subs.safety_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- VEHICLE STATUS --- */ subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); fds[fdsc_count].fd = subs.status_sub; @@ -720,6 +743,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- RATES SETPOINT --- */ + subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + fds[fdsc_count].fd = subs.rates_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- ACTUATOR OUTPUTS --- */ subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); fds[fdsc_count].fd = subs.act_outputs_sub; @@ -762,6 +791,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- CONTROL DEBUG --- */ + subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug)); + fds[fdsc_count].fd = subs.control_debug_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; @@ -774,6 +809,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- AIRSPEED --- */ + subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + fds[fdsc_count].fd = subs.airspeed_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -806,7 +847,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (log_on_start) sdlog2_start_log(); - while (!thread_should_exit) { + while (!main_thread_should_exit) { /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; @@ -816,7 +857,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* handle the poll result */ if (poll_ret < 0) { warnx("ERROR: poll error, stop logging."); - thread_should_exit = true; + main_thread_should_exit = true; } else if (poll_ret > 0) { @@ -833,22 +874,33 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } - /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ + /* --- SAFETY- LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + orb_copy(ORB_ID(actuator_safety), subs.safety_sub, &buf_safety); if (log_when_armed) { - handle_status(&buf_status); + handle_status(&buf_safety); } handled_topics++; } + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + + //if (log_when_armed) { + // handle_status(&buf_safety); + //} + + //handled_topics++; + } + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } - ifds = 1; // Begin from fds[1] again + ifds = 2; // Begin from fds[2] again pthread_mutex_lock(&logbuffer_mutex); @@ -861,11 +913,16 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; - log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; - log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; - log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; - log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed; + // XXX fix this + // log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; + // log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; + // log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; + // log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; + log_msg.body.log_STAT.state = 0; + log_msg.body.log_STAT.flight_mode = 0; + log_msg.body.log_STAT.manual_control_mode = 0; + log_msg.body.log_STAT.manual_sas_mode = 0; + log_msg.body.log_STAT.armed = (unsigned char) buf_safety.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -883,7 +940,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPS.epv = buf.gps_pos.epv_m; log_msg.body.log_GPS.lat = buf.gps_pos.lat; log_msg.body.log_GPS.lon = buf.gps_pos.lon; - log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001; + log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001f; log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s; log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s; log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s; @@ -953,6 +1010,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.roll = buf.att.roll; log_msg.body.log_ATT.pitch = buf.att.pitch; log_msg.body.log_ATT.yaw = buf.att.yaw; + log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; + log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; + log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; LOGBUFFER_WRITE_AND_COUNT(ATT); } @@ -963,9 +1023,20 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; + log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust; LOGBUFFER_WRITE_AND_COUNT(ATSP); } + /* --- RATES SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp); + log_msg.msg_type = LOG_ARSP_MSG; + log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll; + log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch; + log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw; + LOGBUFFER_WRITE_AND_COUNT(ARSP); + } + /* --- ACTUATOR OUTPUTS --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs); @@ -1031,6 +1102,27 @@ int sdlog2_thread_main(int argc, char *argv[]) // TODO not implemented yet } + /* --- CONTROL DEBUG --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); + + log_msg.msg_type = LOG_CTRL_MSG; + log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; + log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; + log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; + log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; + log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; + log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; + log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; + log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; + log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; + log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; + log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; + log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; + + LOGBUFFER_WRITE_AND_COUNT(CTRL); + } + /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); @@ -1046,10 +1138,22 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(RC); } + /* --- AIRSPEED --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed); + log_msg.msg_type = LOG_AIRS_MSG; + log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; + log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; + LOGBUFFER_WRITE_AND_COUNT(AIRS); + } + +#ifdef SDLOG2_DEBUG + printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); +#endif /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { #ifdef SDLOG2_DEBUG - printf("signal %i", logbuffer_count(&lb)); + printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); #endif /* only request write if several packets can be written at once */ pthread_cond_signal(&logbuffer_cond); @@ -1163,10 +1267,10 @@ void handle_command(struct vehicle_command_s *cmd) } } -void handle_status(struct vehicle_status_s *status) +void handle_status(struct actuator_safety_s *safety) { - if (status->flag_system_armed != flag_system_armed) { - flag_system_armed = status->flag_system_armed; + if (safety->armed != flag_system_armed) { + flag_system_armed = safety->armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 40763ee1e..a83a72514 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -60,6 +60,9 @@ struct log_ATT_s { float roll; float pitch; float yaw; + float roll_rate; + float pitch_rate; + float yaw_rate; }; /* --- ATSP - ATTITUDE SET POINT --- */ @@ -68,6 +71,7 @@ struct log_ATSP_s { float roll_sp; float pitch_sp; float yaw_sp; + float thrust_sp; }; /* --- IMU - IMU SENSORS --- */ @@ -156,25 +160,57 @@ struct log_STAT_s { unsigned char battery_warning; }; +/* --- CTRL - CONTROL DEBUG --- */ +#define LOG_CTRL_MSG 11 +struct log_CTRL_s { + float roll_p; + float roll_i; + float roll_d; + float roll_rate_p; + float roll_rate_i; + float roll_rate_d; + float pitch_p; + float pitch_i; + float pitch_d; + float pitch_rate_p; + float pitch_rate_i; + float pitch_rate_d; +}; + /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 11 +#define LOG_RC_MSG 12 struct log_RC_s { float channel[8]; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 12 +#define LOG_OUT0_MSG 13 struct log_OUT0_s { float output[8]; }; + +/* --- AIRS - AIRSPEED --- */ +#define LOG_AIRS_MSG 13 +struct log_AIRS_s { + float indicated_airspeed; + float true_airspeed; +}; + +/* --- ARSP - ATTITUDE RATE SET POINT --- */ +#define LOG_ARSP_MSG 14 +struct log_ARSP_s { + float roll_rate_sp; + float pitch_rate_sp; + float yaw_rate_sp; +}; #pragma pack(pop) /* construct list of all message formats */ static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), - LOG_FORMAT(ATT, "fff", "Roll,Pitch,Yaw"), - LOG_FORMAT(ATSP, "fff", "RollSP,PitchSP,YawSP"), + LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), + LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), @@ -182,8 +218,11 @@ static const struct log_format_s log_formats[] = { 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, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(CTRL, "ffffffffffff", "RP,RI,RD,RRP,RRI,RRD,PP,PI,PD,PRP,PRI,PRD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), 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"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 230060148..04b2cde8b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -160,13 +160,11 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6b6aeedee..560ef6406 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -207,13 +207,11 @@ private: int rc_map_yaw; int rc_map_throttle; - int rc_map_manual_override_sw; - int rc_map_auto_mode_sw; + int rc_map_mode_sw; + int rc_map_return_sw; + int rc_map_mission_sw; - int rc_map_manual_mode_sw; - int rc_map_sas_mode_sw; - int rc_map_rtl_sw; - int rc_map_offboard_ctrl_mode_sw; +// int rc_map_offboard_ctrl_mode_sw; int rc_map_flaps; @@ -254,13 +252,11 @@ private: param_t rc_map_yaw; param_t rc_map_throttle; - param_t rc_map_manual_override_sw; - param_t rc_map_auto_mode_sw; + param_t rc_map_mode_sw; + param_t rc_map_return_sw; + param_t rc_map_mission_sw; - param_t rc_map_manual_mode_sw; - param_t rc_map_sas_mode_sw; - param_t rc_map_rtl_sw; - param_t rc_map_offboard_ctrl_mode_sw; +// param_t rc_map_offboard_ctrl_mode_sw; param_t rc_map_flaps; @@ -460,16 +456,15 @@ Sensors::Sensors() : _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ - _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); - _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); - _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW"); - _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); - _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + +// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -606,33 +601,25 @@ Sensors::parameters_update() warnx("Failed getting throttle chan index"); } - if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { - warnx("Failed getting override sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { - warnx("Failed getting auto mode sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx("Failed getting flaps chan index"); + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { + warnx("Failed getting mode sw chan index"); } - if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { - warnx("Failed getting manual mode sw chan index"); + if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { + warnx("Failed getting return sw chan index"); } - if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) { - warnx("Failed getting rtl sw chan index"); + if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + warnx("Failed getting mission sw chan index"); } - if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) { - warnx("Failed getting sas mode sw chan index"); + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { + warnx("Failed getting flaps chan index"); } - if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { - warnx("Failed getting offboard control mode sw chan index"); - } +// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { +// warnx("Failed getting offboard control mode sw chan index"); +// } if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { warnx("Failed getting mode aux 1 index"); @@ -676,15 +663,13 @@ Sensors::parameters_update() _rc.function[PITCH] = _parameters.rc_map_pitch - 1; _rc.function[YAW] = _parameters.rc_map_yaw - 1; - _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; - _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; + _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; - _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; - _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1; - _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1; - _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; +// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; @@ -1137,10 +1122,10 @@ Sensors::ppm_poll() manual_control.yaw = NAN; manual_control.throttle = NAN; - manual_control.manual_mode_switch = NAN; - manual_control.manual_sas_switch = NAN; - manual_control.return_to_launch_switch = NAN; - manual_control.auto_offboard_input_switch = NAN; + manual_control.mode_switch = NAN; + manual_control.return_switch = NAN; + manual_control.mission_switch = NAN; +// manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; manual_control.aux1 = NAN; @@ -1240,11 +1225,14 @@ Sensors::ppm_poll() manual_control.yaw *= _parameters.rc_scale_yaw; } - /* override switch input */ - manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled); - /* mode switch input */ - manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + + /* land switch input */ + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + + /* land switch input */ + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1256,21 +1244,17 @@ Sensors::ppm_poll() } } - if (_rc.function[MANUAL_MODE] >= 0) { - manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); - } - - if (_rc.function[SAS_MODE] >= 0) { - manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); } - if (_rc.function[RTL] >= 0) { - manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } - if (_rc.function[OFFBOARD_MODE] >= 0) { - manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); - } +// if (_rc.function[OFFBOARD_MODE] >= 0) { +// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); +// } /* aux functions, only assign if valid mapping is present */ if (_rc.function[AUX_1] >= 0) { diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 49315cdc9..3685b2aeb 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -43,7 +43,7 @@ #include <math.h> __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode) + float limit, float diff_filter_factor, uint8_t mode) { pid->kp = kp; pid->ki = ki; @@ -51,15 +51,17 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; - pid->count = 0; - pid->saturated = 0; - pid->last_output = 0; - - pid->sp = 0; - pid->error_previous = 0; - pid->integral = 0; + pid->diff_filter_factor = diff_filter_factor; + pid->count = 0.0f; + pid->saturated = 0.0f; + pid->last_output = 0.0f; + + pid->sp = 0.0f; + pid->error_previous_filtered = 0.0f; + pid->control_previous = 0.0f; + pid->integral = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor) { int ret = 0; @@ -98,6 +100,13 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } + if (isfinite(diff_filter_factor)) { + pid->diff_filter_factor = diff_filter_factor; + + } else { + ret = 1; + } + return ret; } @@ -115,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float * @param dt * @return */ -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d) { /* error = setpoint - actual_position integral = integral + (error*dt) @@ -136,15 +145,15 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - if (isfinite(error)) { // Why is this necessary? DEW - pid->error_previous = error; - } + float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered; // Calculate or measured current error derivative if (pid->mode == PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / dt; +// d = (error_filtered - pid->error_previous_filtered) / dt; + d = error_filtered - pid->error_previous_filtered ; + pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -170,8 +179,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } // Calculate the output. Limit output magnitude to pid->limit - float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); - + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (output > pid->limit) output = pid->limit; if (output < -pid->limit) output = -pid->limit; @@ -180,6 +188,15 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo pid->last_output = output; } + pid->control_previous = pid->last_output; + + // if (isfinite(error)) { // Why is this necessary? DEW + // pid->error_previous = error; + // } + + *ctrl_p = (error * pid->kp); + *ctrl_i = (i * pid->ki); + *ctrl_d = (d * pid->kd); return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index 64d668867..e3d9038cd 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -59,18 +59,20 @@ typedef struct { float intmax; float sp; float integral; - float error_previous; + float error_previous_filtered; + float control_previous; float last_output; float limit; uint8_t mode; + float diff_filter_factor; uint8_t count; uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d); __EXPORT void pid_reset_integral(PID_t *pid); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index e2df31651..b5dafd0ca 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -98,9 +98,15 @@ ORB_DEFINE(rc_channels, struct rc_channels_s); #include "topics/vehicle_command.h" ORB_DEFINE(vehicle_command, struct vehicle_command_s); +#include "topics/vehicle_control_mode.h" +ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s); + #include "topics/vehicle_local_position_setpoint.h" ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); +#include "topics/vehicle_bodyframe_speed_setpoint.h" +ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); + #include "topics/vehicle_global_position_setpoint.h" ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s); @@ -113,12 +119,18 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); +#include "topics/vehicle_control_debug.h" +ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); + #include "topics/offboard_control_setpoint.h" ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); #include "topics/optical_flow.h" ORB_DEFINE(optical_flow, struct optical_flow_s); +#include "topics/filtered_bottom_flow.h" +ORB_DEFINE(filtered_bottom_flow, struct filtered_bottom_flow_s); + #include "topics/omnidirectional_flow.h" ORB_DEFINE(omnidirectional_flow, struct omnidirectional_flow_s); @@ -137,7 +149,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); -ORB_DEFINE(actuator_armed, struct actuator_armed_s); + +#include "topics/actuator_safety.h" +ORB_DEFINE(actuator_safety, struct actuator_safety_s); /* actuator controls, as set by actuators / mixers after limiting */ #include "topics/actuator_controls_effective.h" diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index 745c5bc87..26b967237 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -52,6 +52,9 @@ #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) + struct actuator_controls_s { uint64_t timestamp; float control[NUM_ACTUATOR_CONTROLS]; @@ -63,17 +66,4 @@ ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) - -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool throttle_locked; /**< Set to true if the trottle is locked to zero */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ -}; - -ORB_DECLARE(actuator_armed); - #endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h new file mode 100644 index 000000000..3a107d41a --- /dev/null +++ b/src/modules/uORB/topics/actuator_safety.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file actuator_controls.h + * + * Actuator control topics - mixer inputs. + * + * Values published to these topics are the outputs of the vehicle control + * system, and are expected to be mixed and used to drive the actuators + * (servos, speed controls, etc.) that operate the vehicle. + * + * Each topic can be published by a single controller + */ + +#ifndef TOPIC_ACTUATOR_SAFETY_H +#define TOPIC_ACTUATOR_SAFETY_H + +#include <stdint.h> +#include "../uORB.h" + +/** global 'actuator output is live' control. */ +struct actuator_safety_s { + + uint64_t timestamp; + + bool safety_off; /**< Set to true if safety is off */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */ +}; + +ORB_DECLARE(actuator_safety); + +#endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/uORB/topics/filtered_bottom_flow.h new file mode 100644 index 000000000..ab6de2613 --- /dev/null +++ b/src/modules/uORB/topics/filtered_bottom_flow.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file filtered_bottom_flow.h + * Definition of the filtered bottom flow uORB topic. + */ + +#ifndef TOPIC_FILTERED_BOTTOM_FLOW_H_ +#define TOPIC_FILTERED_BOTTOM_FLOW_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Filtered bottom flow in bodyframe. + */ +struct filtered_bottom_flow_s +{ + uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ + + float sumx; /**< Integrated bodyframe x flow in meters */ + float sumy; /**< Integrated bodyframe y flow in meters */ + + float vx; /**< Flow bodyframe x speed, m/s */ + float vy; /**< Flow bodyframe y Speed, m/s */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(filtered_bottom_flow); + +#endif diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 261a8a4ad..cfee81ea2 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -56,17 +56,17 @@ struct manual_control_setpoint_s { float yaw; /**< rudder / yaw rate / yaw */ float throttle; /**< throttle / collective thrust / altitude */ - float manual_override_switch; /**< manual override mode (mandatory) */ - float auto_mode_switch; /**< auto mode switch (mandatory) */ + float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + float return_switch; /**< land 2 position switch (mandatory): land, no effect */ + float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ /** * Any of the channels below may not be available and be set to NaN * to indicate that it does not contain valid data. */ - float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */ - float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ - float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ - float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ + + // XXX needed or parameter? + //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ float flaps; /**< flap position */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 9dd54df91..a0bb25af4 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -68,18 +68,16 @@ enum RC_CHANNELS_FUNCTION ROLL = 1, PITCH = 2, YAW = 3, - OVERRIDE = 4, - AUTO_MODE = 5, - MANUAL_MODE = 6, - SAS_MODE = 7, - RTL = 8, - OFFBOARD_MODE = 9, - FLAPS = 10, - AUX_1 = 11, - AUX_2 = 12, - AUX_3 = 13, - AUX_4 = 14, - AUX_5 = 15, + MODE = 4, + RETURN = 5, + MISSION = 6, + OFFBOARD_MODE = 7, + FLAPS = 8, + AUX_1 = 9, + AUX_2 = 10, + AUX_3 = 11, + AUX_4 = 12, + AUX_5 = 13, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h new file mode 100644 index 000000000..fbfab09f3 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Samuel Zihlmann <samuezih@ee.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_bodyframe_speed_setpoint.h + * Definition of the bodyframe speed setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_ +#define TOPIC_VEHICLE_BODYFRAME_SPEED_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_bodyframe_speed_setpoint_s +{ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + float vx; /**< in m/s */ + float vy; /**< in m/s */ +// float vz; /**< in m/s */ + float thrust_sp; + float yaw_sp; /**< in radian -PI +PI */ +}; /**< Speed in bodyframe to go to */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_bodyframe_speed_setpoint); + +#endif diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h new file mode 100644 index 000000000..6184284a4 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_control_debug.h + * For debugging purposes to log PID parts of controller + */ + +#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_ +#define TOPIC_VEHICLE_CONTROL_DEBUG_H_ + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ +struct vehicle_control_debug_s +{ + uint64_t timestamp; /**< in microseconds since system start */ + + float roll_p; /**< roll P control part */ + float roll_i; /**< roll I control part */ + float roll_d; /**< roll D control part */ + + float roll_rate_p; /**< roll rate P control part */ + float roll_rate_i; /**< roll rate I control part */ + float roll_rate_d; /**< roll rate D control part */ + + float pitch_p; /**< pitch P control part */ + float pitch_i; /**< pitch I control part */ + float pitch_d; /**< pitch D control part */ + + float pitch_rate_p; /**< pitch rate P control part */ + float pitch_rate_i; /**< pitch rate I control part */ + float pitch_rate_d; /**< pitch rate D control part */ + + float yaw_p; /**< yaw P control part */ + float yaw_i; /**< yaw I control part */ + float yaw_d; /**< yaw D control part */ + + float yaw_rate_p; /**< yaw rate P control part */ + float yaw_rate_i; /**< yaw rate I control part */ + float yaw_rate_d; /**< yaw rate D control part */ + +}; /**< vehicle_control_debug */ + + /** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_debug); + +#endif diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h new file mode 100644 index 000000000..177e30898 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file vehicle_control_mode.h + * Definition of the vehicle_control_mode uORB topic. + * + * All control apps should depend their actions based on the flags set here. + */ + +#ifndef VEHICLE_CONTROL_MODE +#define VEHICLE_CONTROL_MODE + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + + +/** + * state machine / state of vehicle. + * + * Encodes the complete system state and is set by the commander app. + */ +struct vehicle_control_mode_s +{ + uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + + bool flag_system_emergency; + bool flag_preflight_calibration; + + 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_auto_enabled; + 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 speed (implies direction) is controlled */ + bool flag_control_position_enabled; /**< true if position is controlled */ + + bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ + bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_mode); + +#endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 07660614b..2bcd57f4b 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -59,21 +59,58 @@ */ /* State Machine */ -typedef enum -{ - SYSTEM_STATE_PREFLIGHT = 0, - SYSTEM_STATE_STANDBY = 1, - SYSTEM_STATE_GROUND_READY = 2, - SYSTEM_STATE_MANUAL = 3, - SYSTEM_STATE_STABILIZED = 4, - SYSTEM_STATE_AUTO = 5, - SYSTEM_STATE_MISSION_ABORT = 6, - SYSTEM_STATE_EMCY_LANDING = 7, - SYSTEM_STATE_EMCY_CUTOFF = 8, - SYSTEM_STATE_GROUND_ERROR = 9, - SYSTEM_STATE_REBOOT= 10, - -} commander_state_machine_t; +typedef enum { + NAVIGATION_STATE_INIT=0, + NAVIGATION_STATE_MANUAL_STANDBY, + NAVIGATION_STATE_MANUAL, + NAVIGATION_STATE_SEATBELT_STANDBY, + NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_SEATBELT_DESCENT, + NAVIGATION_STATE_AUTO_STANDBY, + NAVIGATION_STATE_AUTO_READY, + NAVIGATION_STATE_AUTO_TAKEOFF, + NAVIGATION_STATE_AUTO_LOITER, + NAVIGATION_STATE_AUTO_MISSION, + NAVIGATION_STATE_AUTO_RTL, + NAVIGATION_STATE_AUTO_LAND +} navigation_state_t; + +typedef enum { + MANUAL_STANDBY = 0, + MANUAL_READY, + MANUAL_IN_AIR +} manual_state_t; + +typedef enum { + ARMING_STATE_INIT = 0, + ARMING_STATE_STANDBY, + ARMING_STATE_ARMED, + ARMING_STATE_ARMED_ERROR, + ARMING_STATE_STANDBY_ERROR, + ARMING_STATE_REBOOT, + ARMING_STATE_IN_AIR_RESTORE +} arming_state_t; + +typedef enum { + HIL_STATE_OFF = 0, + HIL_STATE_ON +} hil_state_t; + +typedef enum { + MODE_SWITCH_MANUAL = 0, + MODE_SWITCH_SEATBELT, + MODE_SWITCH_AUTO +} mode_switch_pos_t; + +typedef enum { + RETURN_SWITCH_NONE = 0, + RETURN_SWITCH_RETURN +} return_switch_pos_t; + +typedef enum { + MISSION_SWITCH_NONE = 0, + MISSION_SWITCH_MISSION +} mission_switch_pos_t; enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, @@ -86,26 +123,6 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ -enum VEHICLE_FLIGHT_MODE { - VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ - VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ - VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ - VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ -}; - -enum VEHICLE_MANUAL_CONTROL_MODE { - VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ - VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ - VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ -}; - -enum VEHICLE_MANUAL_SAS_MODE { - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ - VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ - VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ -}; - /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -134,7 +151,7 @@ enum VEHICLE_TYPE { enum VEHICLE_BATTERY_WARNING { VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ - VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */ + VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */ }; @@ -150,32 +167,55 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ - //uint64_t failsave_highlevel_begin; TO BE COMPLETED +// uint64_t failsave_highlevel_begin; TO BE COMPLETED + + navigation_state_t navigation_state; /**< current system state */ + arming_state_t arming_state; /**< current arming state */ + hil_state_t hil_state; /**< current hil state */ - commander_state_machine_t state_machine; /**< current flight state, main state machine */ - enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ - enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ - enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ 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 */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ /* system flags - these represent the state predicates */ - bool flag_system_armed; /**< true is motors / actuators are armed */ - bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + mode_switch_pos_t mode_switch; + return_switch_pos_t return_switch; + mission_switch_pos_t mission_switch; + + + bool condition_system_emergency; + bool condition_system_in_air_restore; /**< true if we can restore in mid air */ + bool condition_system_sensors_initialized; + bool condition_system_returned_to_home; + bool condition_auto_mission_available; + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_launch_position_valid; /**< indicates a valid launch position */ + bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + bool condition_local_position_valid; + bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + + // bool condition_auto_flight_mode_ok; /**< conditions of auto flight mode apply plus a valid takeoff position lock has been aquired */ + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ + //bool flag_armed; /**< true is motors / actuators are armed */ + //bool flag_safety_off; /**< true if safety is off */ + bool flag_system_emergency; + bool flag_preflight_calibration; - 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 speed (implies direction) is controlled */ - bool flag_control_position_enabled; /**< true if position is controlled */ + // 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_auto_enabled; + // 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 speed (implies direction) is controlled */ + // bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ - bool flag_preflight_accel_calibration; - bool flag_preflight_airspeed_calibration; + // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ + // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ + // bool flag_preflight_accel_calibration; + // bool flag_preflight_airspeed_calibration; bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ @@ -188,7 +228,7 @@ struct vehicle_status_s uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - //bool failsave_highlevel; + bool failsave_highlevel; /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; @@ -205,17 +245,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ - bool flag_local_position_valid; - bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ - bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_valid_launch_position; /**< indicates a valid launch position */ - bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ - bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ - bool flag_safety_present; /**< indicates that a safety switch is present */ - bool flag_safety_safe; /**< safety switch is in safe position */ }; /** diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index 47d3cd948..cc380f94b 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -47,6 +47,9 @@ __EXPORT int reboot_main(int argc, char *argv[]); int reboot_main(int argc, char *argv[]) { + printf("Rebooting now...\n"); + fflush(stdout); + usleep(5000); up_systemreset(); } |