From adb04f632f765f821cb327448339de8af1e49adb Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 22 Nov 2012 20:35:08 -0800 Subject: Local changes to match upstream changes. --- apps/drivers/boards/px4fmu/px4fmu_init.c | 42 ++++++++++++-------------------- 1 file changed, 16 insertions(+), 26 deletions(-) (limited to 'apps/drivers') diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 57ffb77d3..bc047aa4f 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -150,9 +150,7 @@ __EXPORT int nsh_archinitialize(void) int result; /* configure the high-resolution time/callout interface */ -#ifdef CONFIG_HRT_TIMER hrt_init(); -#endif /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION @@ -160,27 +158,21 @@ __EXPORT int nsh_archinitialize(void) #endif /* set up the serial DMA polling */ -#ifdef SERIAL_HAVE_DMA - { - static struct hrt_call serial_dma_call; - struct timespec ts; - - /* - * Poll at 1ms intervals for received bytes that have not triggered - * a DMA event. - */ - ts.tv_sec = 0; - ts.tv_nsec = 1000000; - - hrt_call_every(&serial_dma_call, - ts_to_abstime(&ts), - ts_to_abstime(&ts), - (hrt_callout)stm32_serial_dma_poll, - NULL); - } -#endif - - message("\r\n"); + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); // initial LED state drv_led_start(); @@ -209,8 +201,7 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully initialized SPI port 1\r\n"); -#if defined(CONFIG_STM32_SPI3) - /* Get the SPI port */ + /* Get the SPI port for the microsd slot */ message("[boot] Initializing SPI port 3\n"); spi3 = up_spiinitialize(3); @@ -233,7 +224,6 @@ __EXPORT int nsh_archinitialize(void) } message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); -#endif /* SPI3 */ #ifdef CONFIG_ADC int adc_state = adc_devinit(); -- cgit v1.2.3 From 90b94b50501cd388d6acfba66bb048393860af74 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Nov 2012 11:53:50 +0100 Subject: Ported all mixers to actuator_controls_effective topic, mixers do not output the limited result yet --- apps/drivers/px4fmu/fmu.cpp | 18 +++++++++++++++++- apps/drivers/px4io/px4io.cpp | 18 +++++++++++++++++- apps/mavlink/orb_listener.c | 8 ++++---- apps/uORB/topics/actuator_controls_effective.h | 7 ++++--- 4 files changed, 42 insertions(+), 9 deletions(-) (limited to 'apps/drivers') diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 61dd418f8..ff610b0b9 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -64,6 +64,7 @@ #include #include +#include #include #include @@ -96,6 +97,7 @@ private: int _t_actuators; int _t_armed; orb_advert_t _t_outputs; + orb_advert_t _t_actuators_effective; unsigned _num_outputs; bool _primary_pwm_device; @@ -161,6 +163,7 @@ PX4FMU::PX4FMU() : _t_actuators(-1), _t_armed(-1), _t_outputs(0), + _t_actuators_effective(0), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), @@ -315,6 +318,13 @@ PX4FMU::task_main() _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + /* advertise the effective control inputs */ + actuator_controls_effective_s controls_effective; + memset(&controls_effective, 0, sizeof(controls_effective)); + /* advertise the effective control inputs */ + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), + &controls_effective); + pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; @@ -360,6 +370,11 @@ PX4FMU::task_main() /* do mixing */ _mixers->mix(&outputs.output[0], num_outputs); + // XXX output actual limited values + memcpy(&controls_effective, &_controls, sizeof(controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); + /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { @@ -371,7 +386,7 @@ PX4FMU::task_main() } /* and publish for anyone that cares to see */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); } } @@ -388,6 +403,7 @@ PX4FMU::task_main() } ::close(_t_actuators); + ::close(_t_actuators_effective); ::close(_t_armed); /* make sure servos are off */ diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index bf23c3937..2385385a2 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -70,6 +70,7 @@ #include #include +#include #include #include @@ -102,6 +103,9 @@ private: int _t_actuators; ///< actuator output topic actuator_controls_s _controls; ///< actuator outputs + orb_advert_t _t_actuators_effective; ///< effective actuator controls topic + actuator_controls_effective_s _controls_effective; ///< effective controls + int _t_armed; ///< system armed control topic actuator_armed_s _armed; ///< system armed state @@ -184,6 +188,7 @@ PX4IO::PX4IO() : _task_should_exit(false), _connected(false), _t_actuators(-1), + _t_actuators_effective(-1), _t_armed(-1), _t_outputs(-1), _mixers(nullptr), @@ -319,6 +324,11 @@ PX4IO::task_main() _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + /* advertise the limited control inputs */ + memset(&_controls_effective, 0, sizeof(_controls_effective)); + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1), + &_controls_effective); + /* advertise the mixed control outputs */ memset(&_outputs, 0, sizeof(_outputs)); _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), @@ -372,6 +382,12 @@ PX4IO::task_main() /* XXX is this the right count? */ _mixers->mix(&_outputs.output[0], _max_actuators); + // XXX output actual limited values + memcpy(&_controls_effective, &_controls, sizeof(_controls_effective)); + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective); + + /* convert to PWM values */ for (unsigned i = 0; i < _max_actuators; i++) _outputs.output[i] = 1500 + (600 * _outputs.output[i]); @@ -496,7 +512,7 @@ PX4IO::io_send() cmd.servo_command[i] = _outputs.output[i]; /* publish as we send */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); // XXX relays diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index ef4cd5598..254100b2e 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -575,19 +575,19 @@ l_vehicle_attitude_controls(struct listener *l) mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl0 ", - actuators.control[0]); + actuators.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl1 ", - actuators.control[1]); + actuators.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl2 ", - actuators.control[2]); + actuators.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl3 ", - actuators.control[3]); + actuators.control_effective[3]); } } diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h index 3c72e4cb7..83f082c8a 100644 --- a/apps/uORB/topics/actuator_controls_effective.h +++ b/apps/uORB/topics/actuator_controls_effective.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file actuator_controls.h + * @file actuator_controls_effective.h * * Actuator control topics - mixer inputs. * @@ -48,9 +48,10 @@ #include #include "../uORB.h" +#include "actuator_controls.h" -#define NUM_ACTUATOR_CONTROLS 8 -#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS +#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ struct actuator_controls_effective_s { uint64_t timestamp; -- cgit v1.2.3 From 03076a72ca75917cf62d7889c6c6d0de36866b04 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Dec 2012 10:23:02 +0100 Subject: Added required additional fields: If system is ok to launch (required for LED indicator), if system is ok to override fully by RC (required for multirotors which should not support this), desired PWM output rate in Hz (again required for some multirotors). --- apps/commander/commander.c | 94 ++++++++++++++++-- apps/drivers/px4fmu/fmu.cpp | 10 +- apps/drivers/px4io/px4io.cpp | 108 +++++++++++++++++++-- .../fixedwing_att_control_main.c | 55 +++++++++-- apps/mavlink/mavlink_receiver.c | 24 ++++- apps/px4io/comms.c | 40 +++++--- apps/px4io/controls.c | 36 +++++++ apps/px4io/mixer.c | 14 ++- apps/px4io/protocol.h | 9 +- apps/px4io/px4io.c | 2 + apps/px4io/px4io.h | 31 +++++- apps/px4io/safety.c | 13 ++- apps/uORB/topics/vehicle_status.h | 7 +- 13 files changed, 390 insertions(+), 53 deletions(-) (limited to 'apps/drivers') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7277e9fa4..310a707aa 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -72,10 +72,12 @@ #include #include #include -#include +#include +#include #include #include #include +#include #include #include @@ -1167,6 +1169,8 @@ int commander_thread_main(int argc, char *argv[]) failsafe_lowlevel_timeout_ms = 0; param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + param_t _param_sys_type = param_find("MAV_TYPE"); + /* welcome user */ printf("[commander] I am in command now!\n"); @@ -1199,6 +1203,8 @@ int commander_thread_main(int argc, char *argv[]) /* 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; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1249,9 +1255,15 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps; - memset(&gps, 0, sizeof(gps)); + 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; + + 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)); + uint64_t last_local_position_time = 0; int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; @@ -1262,6 +1274,11 @@ int commander_thread_main(int argc, char *argv[]) struct vehicle_command_s cmd; memset(&cmd, 0, sizeof(cmd)); + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1288,7 +1305,6 @@ int commander_thread_main(int argc, char *argv[]) if (new_data) { orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); orb_check(cmd_sub, &new_data); @@ -1300,6 +1316,46 @@ int commander_thread_main(int argc, char *argv[]) handle_command(stat_pub, ¤t_status, &cmd); } + /* update parameters */ + orb_check(param_changed_sub, &new_data); + if (new_data) { + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + /* update parameters */ + if (!current_status.flag_system_armed) { + current_status.system_type = param_get(_param_sys_type, &(current_status.system_type)); + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == MAV_TYPE_QUADROTOR || + current_status.system_type == MAV_TYPE_HEXAROTOR || + current_status.system_type == MAV_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + } else { + current_status.flag_external_manual_override_ok = true; + } + printf("param changed, val: %d, override: %s\n", current_status.system_type, (current_status.flag_external_manual_override_ok) ? "ON" : "OFF"); + } else { + printf("ARMED, rejecting sys type change\n"); + } + } + + /* update global position estimate */ + orb_check(global_position_sub, &new_data); + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + last_global_position_time = global_position.timestamp; + } + + /* update local position estimate */ + orb_check(local_position_sub, &new_data); + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + last_local_position_time = local_position.timestamp; + } + battery_voltage = sensors.battery_voltage_v; battery_voltage_valid = sensors.battery_voltage_valid; @@ -1406,6 +1462,32 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + 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; + + /* check for global or local position updates, set a timeout of 2s */ + if (hrt_absolute_time() - last_global_position_time < 2000000) { + current_status.flag_vector_flight_mode_ok = true; + current_status.flag_global_position_valid = true; + // XXX check for controller status and home position as well + } + + if (hrt_absolute_time() - last_local_position_time < 2000000) { + current_status.flag_vector_flight_mode_ok = true; + current_status.flag_local_position_valid = true; + // XXX check for controller status and home position as well + } + + /* 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) { + state_changed = true; + } + + + /* Check if last transition deserved an audio event */ // #warning This code depends on state that is no longer? maintained // #if 0 @@ -1669,7 +1751,7 @@ int commander_thread_main(int argc, char *argv[]) buzzer_deinit(); close(sp_man_sub); close(sp_offboard_sub); - close(gps_sub); + close(global_position_sub); close(sensor_sub); close(cmd_sub); diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 61dd418f8..d5c50fbf0 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -331,8 +331,16 @@ PX4FMU::task_main() /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); - if (update_rate_in_ms < 2) + /* reject faster than 500 Hz updates */ + if (update_rate_in_ms < 2) { update_rate_in_ms = 2; + _update_rate = 500; + } + /* reject slower than 50 Hz updates */ + if (update_rate_in_ms > 20) { + update_rate_in_ms = 20; + _update_rate = 50; + } orb_set_interval(_t_actuators, update_rate_in_ms); up_pwm_servo_set_rate(_update_rate); _current_update_rate = _update_rate; diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 456564ba7..868d53cfd 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include @@ -72,6 +73,7 @@ #include #include +#include #include #include @@ -88,8 +90,17 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); + /** + * Set the PWM via serial update rate + * @warning this directly affects CPU load + */ + int set_pwm_rate(int hz); + + bool dump_one; + private: static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; + int _update_rate; ///< serial send rate in Hz int _serial_fd; ///< serial interface to PX4IO hx_stream_t _io_stream; ///< HX protocol stream @@ -103,6 +114,8 @@ private: int _t_armed; ///< system armed control topic actuator_armed_s _armed; ///< system armed state + int _t_vstatus; ///< system / vehicle status + vehicle_status_s _vstatus; ///< overall system state orb_advert_t _to_input_rc; ///< rc inputs from io rc_input_values _input_rc; ///< rc input values @@ -175,6 +188,8 @@ PX4IO *g_dev; PX4IO::PX4IO() : CDev("px4io", "/dev/px4io"), + dump_one(false), + _update_rate(50), _serial_fd(-1), _io_stream(nullptr), _task(-1), @@ -182,6 +197,7 @@ PX4IO::PX4IO() : _connected(false), _t_actuators(-1), _t_armed(-1), + _t_vstatus(-1), _t_outputs(-1), _mixers(nullptr), _primary_pwm_device(false), @@ -256,6 +272,17 @@ PX4IO::init() return OK; } +int +PX4IO::set_pwm_rate(int hz) +{ + if (hz > 0 && hz <= 400) { + _update_rate = hz; + return OK; + } else { + return -EINVAL; + } +} + void PX4IO::task_main_trampoline(int argc, char *argv[]) { @@ -266,6 +293,7 @@ void PX4IO::task_main() { log("starting"); + int update_rate_in_ms; /* open the serial port */ _serial_fd = ::open("/dev/ttyS2", O_RDWR); @@ -303,12 +331,20 @@ PX4IO::task_main() _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1)); /* convert the update rate in hz to milliseconds, rounding down if necessary */ - //int update_rate_in_ms = int(1000 / _update_rate); - orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */ + update_rate_in_ms = int(1000 / _update_rate); + orb_set_interval(_t_actuators, update_rate_in_ms); _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ + + /* advertise the limited control inputs */ + memset(&_controls_effective, 0, sizeof(_controls_effective)); + _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1), + &_controls_effective); + /* advertise the mixed control outputs */ memset(&_outputs, 0, sizeof(_outputs)); _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), @@ -319,13 +355,15 @@ PX4IO::task_main() _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); /* poll descriptor */ - pollfd fds[3]; + pollfd fds[4]; fds[0].fd = _serial_fd; fds[0].events = POLLIN; fds[1].fd = _t_actuators; fds[1].events = POLLIN; fds[2].fd = _t_armed; fds[2].events = POLLIN; + fds[3].fd = _t_vstatus; + fds[3].events = POLLIN; log("ready"); @@ -354,7 +392,8 @@ PX4IO::task_main() if (fds[1].revents & POLLIN) { /* get controls */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* mix */ if (_mixers != nullptr) { @@ -362,8 +401,19 @@ PX4IO::task_main() _mixers->mix(&_outputs.output[0], _max_actuators); /* convert to PWM values */ - for (unsigned i = 0; i < _max_actuators; i++) - _outputs.output[i] = 1500 + (600 * _outputs.output[i]); + for (unsigned i = 0; i < _max_actuators; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (isfinite(_outputs.output[i]) && _outputs.output[i] >= -1.0f && _outputs.output[i] <= 1.0f) { + _outputs.output[i] = 1500 + (600 * _outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + _outputs.output[i] = 900; + } + } /* and flag for update */ _send_needed = true; @@ -375,6 +425,11 @@ PX4IO::task_main() orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed); _send_needed = true; } + + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus); + _send_needed = true; + } } /* send an update to IO if required */ @@ -499,8 +554,14 @@ PX4IO::io_send() // XXX relays - /* armed and not locked down */ + /* armed and not locked down -> arming ok */ cmd.arm_ok = (_armed.armed && !_armed.lockdown); + /* indicate that full autonomous position control / vector flight mode is available */ + cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok; + /* allow manual override on IO (not allowed for multirotors or other systems with SAS) */ + cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok; + /* set desired PWM output rate */ + cmd.servo_rate = _update_rate; ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); if (ret) @@ -665,6 +726,29 @@ test(void) exit(0); } +void +monitor(void) +{ + unsigned cancels = 3; + printf("Hit three times to exit monitor mode\n"); + + for (;;) { + pollfd fds[1]; + + fds[0].fd = 0; + fds[0].events = POLLIN; + poll(fds, 1, 500); + + if (fds[0].revents == POLLIN) { + int c; + read(0, &c, 1); + if (cancels-- == 0) + exit(0); + } + + if (g_dev != nullptr) + g_dev->dump_one = true; + } } int @@ -686,6 +770,16 @@ px4io_main(int argc, char *argv[]) errx(1, "driver init failed"); } + /* look for the optional pwm update rate for the supported modes */ + if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if (argc > 2 + 1) { + g_dev->set_pwm_rate(atoi(argv[2 + 1])); + } else { + fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + return 1; + } + } + exit(0); } diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 38f58ac33..479ffd9a3 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -182,15 +182,45 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) gyro[1] = att.pitchspeed; gyro[2] = att.yawspeed; - /* Control */ + /* control */ if (vstatus.state_machine == SYSTEM_STATE_AUTO) { - /* Attitude Control */ - fixedwing_att_control_attitude(&att_sp, - &att, - &rates_sp); + /* attitude control */ + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); + /* publish rate setpoint */ + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); - /* Attitude Rate Control */ + /* angular rate control */ + fixedwing_att_control_rates(&rates_sp, gyro, &actuators); + + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; + + } else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) { + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if(vstatus.rc_signal_lost) { + + // XXX define failsafe throttle param + //param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.3f; + att_sp.pitch_body = 0.0f; + att_sp.thrust = 0.5f; + + // 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(); + } + + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); + + /* angular rate control */ fixedwing_att_control_rates(&rates_sp, gyro, &actuators); //REMOVEME XXX @@ -207,8 +237,17 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) actuators.control[3] = manual_sp.throttle; } - /* publish output */ - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + /* publish rates */ + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); + + /* sanity check and publish actuator outputs */ + if (isfinite(actuators.control[0]) && + isfinite(actuators.control[1]) && + isfinite(actuators.control[2]) && + isfinite(actuators.control[3])) + { + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + } } printf("[fixedwing_att_control] exiting, stopping all motors.\n"); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index dd011aeed..a8894f059 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -261,8 +261,8 @@ handle_message(mavlink_message_t *msg) offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1] / (float)UINT16_MAX; if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { ml_armed = false; @@ -298,6 +298,26 @@ handle_message(mavlink_message_t *msg) mavlink_hil_state_t hil_state; mavlink_msg_hil_state_decode(msg, &hil_state); + /* Calculate Rotation Matrix */ + //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode + + if (mavlink_system.type == MAV_TYPE_FIXED_WING) { + //TODO: assuming low pitch and roll values for now + hil_attitude.R[0][0] = cosf(hil_state.yaw); + hil_attitude.R[0][1] = sinf(hil_state.yaw); + hil_attitude.R[0][2] = 0.0f; + + hil_attitude.R[1][0] = -sinf(hil_state.yaw); + hil_attitude.R[1][1] = cosf(hil_state.yaw); + hil_attitude.R[1][2] = 0.0f; + + hil_attitude.R[2][0] = 0.0f; + hil_attitude.R[2][1] = 0.0f; + hil_attitude.R[2][2] = 1.0f; + + hil_attitude.R_valid = true; + } + hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 40ea38cf7..c522325dd 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -130,15 +130,10 @@ comms_main(void) last_report_time = now; /* populate the report */ - for (int i = 0; i < system_state.rc_channels; i++) + for (unsigned i = 0; i < system_state.rc_channels; i++) { report.rc_channel[i] = system_state.rc_channel_data[i]; - - if (system_state.sbus_input_ok || system_state.dsm_input_ok || system_state.ppm_input_ok) { - report.channel_count = system_state.rc_channels; - } else { - report.channel_count = 0; } - + report.channel_count = system_state.rc_channels; report.armed = system_state.armed; /* and send it */ @@ -174,26 +169,41 @@ comms_handle_command(const void *buffer, size_t length) irqstate_t flags = irqsave(); /* fetch new PWM output values */ - for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) + for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) { system_state.fmu_channel_data[i] = cmd->servo_command[i]; + } - /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ - if(system_state.arm_ok && !cmd->arm_ok) { + /* if IO is armed and FMU gets disarmed, IO must also disarm */ + if (system_state.arm_ok && !cmd->arm_ok) { system_state.armed = false; } system_state.arm_ok = cmd->arm_ok; - system_state.mixer_use_fmu = true; + system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok; + system_state.manual_override_ok = cmd->manual_override_ok; + system_state.mixer_fmu_available = true; system_state.fmu_data_received = true; + /* set PWM update rate if changed (after limiting) */ + uint16_t new_servo_rate = cmd->servo_rate; - /* handle changes signalled by FMU */ -// if (!system_state.arm_ok && system_state.armed) -// system_state.armed = false; + /* reject faster than 500 Hz updates */ + if (new_servo_rate > 500) { + new_servo_rate = 500; + } + /* reject slower than 50 Hz updates */ + if (new_servo_rate < 50) { + new_servo_rate = 50; + } + if (system_state.servo_rate != new_servo_rate) { + up_pwm_servo_set_rate(new_servo_rate); + system_state.servo_rate = new_servo_rate; + } /* XXX do relay changes here */ - for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) + for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { system_state.relays[i] = cmd->relay_state[i]; + } irqrestore(flags); } diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index d4eace3df..03c8182b2 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -78,7 +78,43 @@ controls_main(void) if (fds[0].revents & POLLIN) dsm_input(); if (fds[1].revents & POLLIN) +<<<<<<< Updated upstream sbus_input(); +======= + locked |= sbus_input(); + + /* + * If we don't have lock from one of the serial receivers, + * look for PPM. It shares an input with S.bus, so there's + * a possibility it will mis-parse an S.bus frame. + * + * XXX each S.bus frame will cause a PPM decoder interrupt + * storm (lots of edges). It might be sensible to actually + * disable the PPM decoder completely if we have an alternate + * receiver lock. + */ + if (!locked) + ppm_input(); + + /* check for manual override status */ + if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) { + /* force manual input override */ + system_state.mixer_manual_override = true; + } else { + /* override not engaged, use FMU */ + system_state.mixer_manual_override = false; + } + + /* + * If we haven't seen any new control data in 200ms, assume we + * have lost input and tell FMU. + */ + if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { + + /* set the number of channels to zero - no inputs */ + system_state.rc_channels = 0; + system_state.rc_lost = true; +>>>>>>> Stashed changes /* XXX do ppm processing, bypass mode, etc. here */ diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index 483e9fe4d..4e0aecdd7 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -97,7 +97,11 @@ mixer_tick(void) /* * Decide which set of inputs we're using. */ +<<<<<<< Updated upstream if (system_state.mixer_use_fmu) { +======= + if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { +>>>>>>> Stashed changes /* we have recent control data from the FMU */ control_count = PX4IO_OUTPUT_CHANNELS; control_values = &system_state.fmu_channel_data[0]; @@ -108,20 +112,24 @@ mixer_tick(void) /* too many frames without FMU input, time to go to failsafe */ if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) { - system_state.mixer_use_fmu = false; + system_state.mixer_manual_override = true; + system_state.mixer_fmu_available = false; } } else { fmu_input_drops = 0; system_state.fmu_data_received = false; } - } else if (system_state.rc_channels > 0) { + } else if (system_state.rc_channels > 0 && system_state.manual_override_ok) { /* we have control data from an R/C input */ control_count = system_state.rc_channels; control_values = &system_state.rc_channel_data[0]; - } else { /* we have no control input */ +<<<<<<< Updated upstream +======= + // XXX builtin failsafe would activate here +>>>>>>> Stashed changes control_count = 0; } /* diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index c704b1201..0bf6d4997 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -52,9 +52,12 @@ struct px4io_command { uint16_t f2i_magic; #define F2I_MAGIC 0x636d - uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; - bool relay_state[PX4IO_RELAY_CHANNELS]; - bool arm_ok; + uint16_t servo_command[PX4IO_OUTPUT_CHANNELS]; /**< servo output channels */ + uint16_t servo_rate; /**< PWM output rate in Hz */ + bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */ + bool arm_ok; /**< FMU allows full arming */ + bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */ + bool manual_override_ok; /**< if true, IO performs a direct manual override */ }; /* config message from FMU to IO */ diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 77524797f..0eed5b011 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -60,6 +60,8 @@ int user_start(int argc, char *argv[]) { /* reset all to zero */ memset(&system_state, 0, sizeof(system_state)); + /* default to 50 Hz PWM outputs */ + system_state.servo_rate = 50; /* configure the high-resolution time/callout interface */ hrt_init(); diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 483b9bcc8..8a6cb48a9 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -69,8 +69,14 @@ struct sys_state_s { +<<<<<<< Updated upstream bool armed; /* IO armed */ bool arm_ok; /* FMU says OK to arm */ +======= + bool armed; /* IO armed */ + bool arm_ok; /* FMU says OK to arm */ + uint16_t servo_rate; /* output rate of servos in Hz */ +>>>>>>> Stashed changes bool ppm_input_ok; /* valid PPM input data */ bool dsm_input_ok; /* valid Spektrum DSM data */ @@ -96,7 +102,7 @@ struct sys_state_s /* * If true, we are using the FMU controls. */ - bool mixer_use_fmu; + bool mixer_manual_override; /* * If true, state that should be reported to FMU has been updated. @@ -113,6 +119,29 @@ struct sys_state_s * in the config packet. */ uint8_t serial_rx_mode; +<<<<<<< Updated upstream +======= + + /** + * If true, the RC signal has been lost for more than a timeout interval + */ + bool rc_lost; + + /** + * If true, the connection to FMU has been lost for more than a timeout interval + */ + bool fmu_lost; + + /** + * If true, FMU is ready for autonomous position control. Only used for LED indication + */ + bool vector_flight_mode_ok; + + /** + * If true, IO performs an on-board manual override with the RC channel values + */ + bool manual_override_ok; +>>>>>>> Stashed changes }; extern struct sys_state_s system_state; diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 60d20905a..52facfb61 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -63,10 +63,11 @@ static unsigned counter; /* * Define the various LED flash sequences for each system state. */ -#define LED_PATTERN_SAFE 0xffff // always on -#define LED_PATTERN_FMU_ARMED 0x4444 // slow blinking -#define LED_PATTERN_IO_ARMED 0x5555 // fast blinking -#define LED_PATTERN_IO_FMU_ARMED 0x5050 // long off then double blink +#define LED_PATTERN_SAFE 0xffff /**< always on */ +#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0x3000 /**< always on with short break */ +#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */ +#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */ +#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */ static unsigned blink_counter = 0; @@ -139,6 +140,8 @@ safety_check_button(void *arg) } } else if (system_state.arm_ok) { pattern = LED_PATTERN_FMU_ARMED; + } else if (system_state.vector_flight_mode_ok) { + pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK; } /* Turn the LED on if we have a 1 at the current bit position */ @@ -165,7 +168,7 @@ failsafe_blink(void *arg) static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ - if (!system_state.mixer_use_fmu) { + if (!system_state.mixer_fmu_available) { failsafe = !failsafe; } else { failsafe = false; diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 23172d7cf..d69befe96 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -116,6 +116,7 @@ struct vehicle_status_s 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_ATTITUDE_MODE attitute_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ + int32_t system_type; /**< system type, inspired by MAVLinks MAV_TYPE enum */ // uint8_t mode; @@ -165,8 +166,10 @@ struct vehicle_status_s uint16_t errors_count4; // bool remote_manual; /**< set to true by the commander when the manual-switch on the remote is set to manual */ - bool gps_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_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_external_manual_override_ok; }; /** -- cgit v1.2.3 From bc3b66043f57adebdef3980f3a113d2d5362416a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Dec 2012 11:34:51 +0100 Subject: Cleaned up HIL on FMU / IO combo --- apps/commander/commander.c | 16 ++++++++++++++-- apps/drivers/hil/hil.cpp | 1 - apps/drivers/px4io/px4io.cpp | 7 +++---- apps/mavlink/orb_listener.c | 19 ++----------------- apps/sensors/sensors.cpp | 4 ++-- 5 files changed, 21 insertions(+), 26 deletions(-) (limited to 'apps/drivers') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 43219c9f9..f61fd053c 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1268,6 +1268,8 @@ int commander_thread_main(int argc, char *argv[]) int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); + sensors.battery_voltage_v = 0.0f; + sensors.battery_voltage_valid = false; /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1305,7 +1307,13 @@ int commander_thread_main(int argc, char *argv[]) if (new_data) { orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + + orb_check(sensor_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } else { + sensors.battery_voltage_valid = false; + } orb_check(cmd_sub, &new_data); if (new_data) { @@ -1434,7 +1442,11 @@ int commander_thread_main(int argc, char *argv[]) /* Check battery voltage */ /* write to sys_status */ - current_status.voltage_battery = battery_voltage; + if (battery_voltage_valid) { + current_status.voltage_battery = battery_voltage; + } else { + current_status.voltage_battery = 0.0f; + } /* if battery voltage is getting lower, warn using buzzer, etc. */ if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 67b16aa42..cb4a48fab 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -382,7 +382,6 @@ HIL::task_main() /* this would be bad... */ if (ret < 0) { log("poll error %d", errno); - usleep(1000000); continue; } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 1fb65413a..695304926 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -410,7 +410,6 @@ PX4IO::task_main() orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &_controls_effective); - /* convert to PWM values */ for (unsigned i = 0; i < _max_actuators; i++) { /* last resort: catch NaN, INF and out-of-band errors */ @@ -426,6 +425,9 @@ PX4IO::task_main() } } + /* publish actuator outputs */ + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); + /* and flag for update */ _send_needed = true; } @@ -570,9 +572,6 @@ PX4IO::io_send() for (unsigned i = 0; i < _max_actuators; i++) cmd.servo_command[i] = _outputs.output[i]; - /* publish as we send */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs); - // XXX relays /* armed and not locked down -> arming ok */ diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index b0aa401fc..e763e642a 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -506,28 +506,13 @@ l_actuator_outputs(struct listener *l) mavlink_mode, 0); } else { - float rudder, throttle; - - /* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */ - - // XXX very ugly, needs rework - if (isfinite(act_outputs.output[3]) - && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) { - /* throttle is fourth output */ - rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; - throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f); - } else { - /* only three outputs, put throttle on position 4 / index 3 */ - rudder = 0; - throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f); - } mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 600.0f, (act_outputs.output[1] - 1500.0f) / 600.0f, - rudder, - throttle, + 0.0f/*(act_outputs.output[2] - 1500.0f) / 600.0f*/, + (act_outputs.output[2] - 900.0f) / 1200.0f, (act_outputs.output[4] - 1500.0f) / 600.0f, (act_outputs.output[5] - 1500.0f) / 600.0f, (act_outputs.output[6] - 1500.0f) / 600.0f, diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 94dbd0b21..bcc383330 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1001,9 +1001,9 @@ Sensors::ppm_poll() // XXX hardcoded for testing purposes, replace with inverted delta mixer from ROMFS /* roll input - mixed roll and pitch channels */ - manual_control.roll = -0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled); + manual_control.roll = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled - _rc.chan[_rc.function[PITCH]].scaled); /* pitch input - mixed roll and pitch channels */ - manual_control.pitch = -0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled); + manual_control.pitch = 0.5f * (_rc.chan[_rc.function[ROLL]].scaled + _rc.chan[_rc.function[PITCH]].scaled); /* yaw input - stick right is positive and positive rotation */ manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled; /* throttle input */ -- cgit v1.2.3 From 4676b71d8ade5b9ce27e63f1d204b8ffed58b325 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 19 Dec 2012 14:19:11 +0100 Subject: Cleanup in ADC driver, re-add all inputs that are present --- apps/drivers/boards/px4fmu/px4fmu_adc.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'apps/drivers') diff --git a/apps/drivers/boards/px4fmu/px4fmu_adc.c b/apps/drivers/boards/px4fmu/px4fmu_adc.c index 987894163..b55af486d 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_adc.c +++ b/apps/drivers/boards/px4fmu/px4fmu_adc.c @@ -67,10 +67,10 @@ /* Identifying number of each ADC channel: Variable Resistor. */ #ifdef CONFIG_STM32_ADC3 -static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11};// , 12, 13}; ADC12 and 13 are used by MPU on v1.5 boards +static const uint8_t g_chanlist[ADC3_NCHANNELS] = {10, 11, 12, 13}; /* Configurations of pins used byte each ADC channels */ -static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11}; // ADC12 and 13 are used by MPU on v1.5 boards, GPIO_ADC3_IN12, GPIO_ADC3_IN13}; +static const uint32_t g_pinlist[ADC3_NCHANNELS] = {GPIO_ADC3_IN10, GPIO_ADC3_IN11, GPIO_ADC3_IN12, GPIO_ADC3_IN13}; #endif /************************************************************************************ -- cgit v1.2.3 From 5b92c517779500d79e6e5f5cff48336550ce5edb Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 20 Dec 2012 21:31:02 -0800 Subject: Initial implementation of application access to the PX4IO relays. --- apps/drivers/drv_pwm_output.h | 2 +- apps/drivers/px4io/px4io.cpp | 32 +++++++++++++++++++++++++++++++- apps/px4io/comms.c | 25 +++++++++++++++++++++---- 3 files changed, 53 insertions(+), 6 deletions(-) (limited to 'apps/drivers') diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 97033f2cd..b2fee65ac 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -95,7 +95,7 @@ ORB_DECLARE(output_pwm); * Note that ioctls and ObjDev updates should not be mixed, as the * behaviour of the system in this case is not defined. */ -#define _PWM_SERVO_BASE 0x2700 +#define _PWM_SERVO_BASE 0x2a00 /** arm all servo outputs handle by this driver */ #define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 456564ba7..d662e634f 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include @@ -114,6 +115,8 @@ private: bool _primary_pwm_device; ///< true if we are the default PWM output + uint32_t _relays; ///< state of the PX4IO relays, one bit per relay + volatile bool _switch_armed; ///< PX4IO switch armed state // XXX how should this work? @@ -185,6 +188,7 @@ PX4IO::PX4IO() : _t_outputs(-1), _mixers(nullptr), _primary_pwm_device(false), + _relays(0), _switch_armed(false), _send_needed(false), _config_needed(false) @@ -497,7 +501,9 @@ PX4IO::io_send() /* publish as we send */ orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs); - // XXX relays + /* update relays */ + for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) + cmd.relay_state[i] = (_relays & (1<< i)) ? true : false; /* armed and not locked down */ cmd.arm_ok = (_armed.armed && !_armed.lockdown); @@ -559,6 +565,30 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) *(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)]; break; + case GPIO_RESET: + _relays = 0; + _send_needed = true; + break; + + case GPIO_SET: + case GPIO_CLEAR: + /* make sure only valid bits are being set */ + if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) { + ret = EINVAL; + break; + } + if (cmd == GPIO_SET) { + _relays |= arg; + } else { + _relays &= ~arg; + } + _send_needed = true; + break; + + case GPIO_GET: + *(uint32_t *)arg = _relays; + break; + case MIXERIOCGETOUTPUTCOUNT: *(unsigned *)arg = _max_actuators; break; diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 40ea38cf7..ffb7fd679 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -178,7 +178,7 @@ comms_handle_command(const void *buffer, size_t length) system_state.fmu_channel_data[i] = cmd->servo_command[i]; /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */ - if(system_state.arm_ok && !cmd->arm_ok) { + if (system_state.arm_ok && !cmd->arm_ok) { system_state.armed = false; } @@ -191,9 +191,26 @@ comms_handle_command(const void *buffer, size_t length) // if (!system_state.arm_ok && system_state.armed) // system_state.armed = false; - /* XXX do relay changes here */ - for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) - system_state.relays[i] = cmd->relay_state[i]; + /* handle relay state changes here */ + for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) { + if (system_state.relays[i] != cmd->relay_state[i]) { + switch (i) { + case 0: + POWER_ACC1(cmd->relay_state[i]); + break; + case 1: + POWER_ACC2(cmd->relay_state[i]); + break; + case 2: + POWER_RELAY1(cmd->relay_state[i]); + break; + case 3: + POWER_RELAY2(cmd->relay_state[i]); + break; + } + } + system_state.relays[i] != cmd->relay_state[i] + } irqrestore(flags); } -- cgit v1.2.3 From 4cf2266b79a28445ad0b493c36cf125081900423 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Dec 2012 00:47:52 +0100 Subject: Robustified actuator output topic, added number of mixed outputs --- apps/drivers/hil/hil.cpp | 27 +++++++++++++++++++-------- apps/drivers/px4fmu/fmu.cpp | 21 ++++++++++++++++++--- apps/drivers/px4io/px4io.cpp | 10 +++++++--- apps/mavlink/orb_listener.c | 18 ++++++++++++++++-- apps/uORB/topics/actuator_outputs.h | 5 +++-- 5 files changed, 63 insertions(+), 18 deletions(-) (limited to 'apps/drivers') diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index cb4a48fab..276a642fd 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -68,11 +68,11 @@ #include #include #include -// #include +#include +#include #include #include -#include #include #include @@ -395,16 +395,27 @@ HIL::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - - /* output to the servo */ - // up_pwm_servo_set(i, outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < (unsigned)outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } } /* and publish for anyone that cares to see */ diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 285516ed9..a672bd2fb 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -382,7 +383,8 @@ PX4FMU::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs.output[0], num_outputs); + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); // XXX output actual limited values memcpy(&controls_effective, &_controls, sizeof(controls_effective)); @@ -392,8 +394,21 @@ PX4FMU::task_main() /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + isfinite(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } /* output to the servo */ up_pwm_servo_set(i, outputs.output[i]); diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 695304926..701955298 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -402,8 +402,8 @@ PX4IO::task_main() /* mix */ if (_mixers != nullptr) { - /* XXX is this the right count? */ - _mixers->mix(&_outputs.output[0], _max_actuators); + _outputs.timestamp = hrt_absolute_time(); + _outputs.noutputs = _mixers->mix(&_outputs.output[0], _max_actuators); // XXX output actual limited values memcpy(&_controls_effective, &_controls, sizeof(_controls_effective)); @@ -413,7 +413,11 @@ PX4IO::task_main() /* convert to PWM values */ for (unsigned i = 0; i < _max_actuators; i++) { /* last resort: catch NaN, INF and out-of-band errors */ - if (isfinite(_outputs.output[i]) && _outputs.output[i] >= -1.0f && _outputs.output[i] <= 1.0f) { + if (i < _outputs.noutputs && + isfinite(_outputs.output[i]) && + _outputs.output[i] >= -1.0f && + _outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ _outputs.output[i] = 1500 + (600 * _outputs.output[i]); } else { /* diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index e763e642a..971ba6a8e 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -507,12 +507,26 @@ l_actuator_outputs(struct listener *l) 0); } else { + /* + * Catch the case where no rudder is in use and throttle is not + * on output four + */ + float rudder, throttle; + + if (act_outputs.noutputs < 4) { + rudder = 0.0f; + throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; + } else { + rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; + throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; + } + mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 600.0f, (act_outputs.output[1] - 1500.0f) / 600.0f, - 0.0f/*(act_outputs.output[2] - 1500.0f) / 600.0f*/, - (act_outputs.output[2] - 900.0f) / 1200.0f, + rudder, + throttle, (act_outputs.output[4] - 1500.0f) / 600.0f, (act_outputs.output[5] - 1500.0f) / 600.0f, (act_outputs.output[6] - 1500.0f) / 600.0f, diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h index accd560af..bbe429073 100644 --- a/apps/uORB/topics/actuator_outputs.h +++ b/apps/uORB/topics/actuator_outputs.h @@ -53,8 +53,9 @@ #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ struct actuator_outputs_s { - uint64_t timestamp; - float output[NUM_ACTUATOR_OUTPUTS]; + uint64_t timestamp; /**< output timestamp in us since system boot */ + float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ + int noutputs; /**< valid outputs */ }; /* actuator output sets; this list can be expanded as more drivers emerge */ -- cgit v1.2.3 From 7526dd46a2ace594cbbb2c6ad9e9fa53c67c5ca8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Dec 2012 19:01:00 +0100 Subject: Added header for common priority bands --- apps/drivers/px4io/px4io.cpp | 3 ++- apps/systemlib/scheduling_priorities.h | 48 ++++++++++++++++++++++++++++++++++ 2 files changed, 50 insertions(+), 1 deletion(-) create mode 100644 apps/systemlib/scheduling_priorities.h (limited to 'apps/drivers') diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 701955298..0ea1218e3 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -70,6 +70,7 @@ #include #include #include +#include #include #include @@ -255,7 +256,7 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); return -errno; diff --git a/apps/systemlib/scheduling_priorities.h b/apps/systemlib/scheduling_priorities.h new file mode 100644 index 000000000..be1dbfcd8 --- /dev/null +++ b/apps/systemlib/scheduling_priorities.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +/* SCHED_PRIORITY_MAX */ +#define SCHED_PRIORITY_FAST_DRIVER SCHED_PRIORITY_MAX +#define SCHED_PRIORITY_WATCHDOG (SCHED_PRIORITY_MAX - 5) +#define SCHED_PRIORITY_ACTUATOR_OUTPUTS (SCHED_PRIORITY_MAX - 15) +#define SCHED_PRIORITY_ATTITUDE_CONTROL (SCHED_PRIORITY_MAX - 25) +#define SCHED_PRIORITY_SLOW_DRIVER (SCHED_PRIORITY_MAX - 35) +#define SCHED_PRIORITY_POSITION_CONTROL (SCHED_PRIORITY_MAX - 40) +/* SCHED_PRIORITY_DEFAULT */ +#define SCHED_PRIORITY_LOGGING (SCHED_PRIORITY_DEFAULT - 10) +#define SCHED_PRIORITY_PARAMS (SCHED_PRIORITY_DEFAULT - 15) +/* SCHED_PRIORITY_IDLE */ -- cgit v1.2.3 From 38a1076a33bd30eae05882460fcd69ae1a6aff4b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 28 Dec 2012 13:12:27 +0100 Subject: Cleaned up attitude control in HIL, implemented very simple guided / stabilized mode with just attitude stabilization --- apps/drivers/hil/hil.cpp | 3 -- .../fixedwing_att_control_main.c | 48 +++++++++++++++++++++- 2 files changed, 46 insertions(+), 5 deletions(-) (limited to 'apps/drivers') diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 276a642fd..3c6355d6e 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -429,9 +429,6 @@ HIL::task_main() /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - - /* update PWM servo armed status */ - // up_pwm_servo_arm(aa.armed); } } diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index 9c450e68e..5ededa2e3 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -198,6 +198,48 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* set flaps to zero */ actuators.control[4] = 0.0f; + } else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) { + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if(vstatus.rc_signal_lost) { + + // XXX define failsafe throttle param + //param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.3f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0; + att_sp.thrust = 0.4f; + + // 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(); + + // XXX: Stop copying setpoint / reference from bus, instead keep position + // and mix RC inputs in. + // XXX: For now just stabilize attitude, not anything else + // proper implementation should do stabilization in position controller + // and just check for stabilized or auto state + + /* 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.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { @@ -208,7 +250,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) //param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.roll_body = 0.3f; att_sp.pitch_body = 0.0f; - att_sp.thrust = 0.5f; + att_sp.thrust = 0.4f; + att_sp.yaw_body = 0; // XXX disable yaw control, loiter @@ -218,9 +261,10 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual_sp.pitch; att_sp.yaw_body = 0; att_sp.thrust = manual_sp.throttle; - att_sp.timestamp = hrt_absolute_time(); } + att_sp.timestamp = hrt_absolute_time(); + /* attitude control */ fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); -- cgit v1.2.3