diff options
author | Julian Oes <julian@oes.ch> | 2013-08-15 14:04:46 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-08-15 14:04:46 +0200 |
commit | 56575eb068879beb68b3730ca6d3bb3755d6960a (patch) | |
tree | 1b7b4c028c2b936db81310e82a8ef84adc125e3d /src/drivers | |
parent | 50cf1c01b701fced6437dfe574fd09cd312b9f15 (diff) | |
parent | 561ec495b7df5b3ff4536d16d1389d1f02affd06 (diff) | |
download | px4-firmware-56575eb068879beb68b3730ca6d3bb3755d6960a.tar.gz px4-firmware-56575eb068879beb68b3730ca6d3bb3755d6960a.tar.bz2 px4-firmware-56575eb068879beb68b3730ca6d3bb3755d6960a.zip |
Merge remote-tracking branch 'px4/new_state_machine_drton' into fmuv2_bringup_new_state_machine_drton
Conflicts:
src/drivers/blinkm/blinkm.cpp
src/drivers/px4io/px4io.cpp
src/modules/commander/state_machine_helper.c
src/modules/px4iofirmware/protocol.h
src/modules/px4iofirmware/registers.c
src/modules/systemlib/systemlib.h
src/systemcmds/reboot/reboot.c
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/ardrone_interface/ardrone_interface.c | 10 | ||||
-rw-r--r-- | src/drivers/blinkm/blinkm.cpp | 79 | ||||
-rw-r--r-- | src/drivers/drv_pwm_output.h | 5 | ||||
-rw-r--r-- | src/drivers/hil/hil.cpp | 1 | ||||
-rw-r--r-- | src/drivers/mkblctrl/mkblctrl.cpp | 15 | ||||
-rw-r--r-- | src/drivers/px4fmu/fmu.cpp | 15 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 363 | ||||
-rw-r--r-- | src/drivers/stm32/tone_alarm/tone_alarm.cpp | 29 |
8 files changed, 392 insertions, 125 deletions
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 735bdb41a..b88f61ce8 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -53,8 +53,10 @@ #include <sys/prctl.h> #include <drivers/drv_hrt.h> #include <uORB/uORB.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/safety.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/vehicle_control_mode.h> #include <systemlib/systemlib.h> @@ -243,16 +245,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int led_counter = 0; /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); struct actuator_armed_s armed; + //XXX is this necessairy? armed.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)); printf("[ardrone_interface] Motors initialized - ready.\n"); @@ -322,8 +322,6 @@ 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); diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index b4c5fa06e..490002254 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -116,6 +116,8 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/actuator_armed.h> #include <uORB/topics/vehicle_gps_position.h> static const float MAX_CELL_VOLTAGE = 4.3f; @@ -375,7 +377,9 @@ BlinkM::led() { static int vehicle_status_sub_fd; + static int vehicle_control_mode_sub_fd; static int vehicle_gps_position_sub_fd; + static int actuator_armed_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -386,6 +390,8 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; + static int no_data_vehicle_control_mode = 0; + static int no_data_actuator_armed = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -398,6 +404,12 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); + vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(vehicle_control_mode_sub_fd, 1000); + + actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(actuator_armed_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 +464,16 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; + struct vehicle_control_mode_s vehicle_control_mode; + struct actuator_armed_s actuator_armed; 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_vehicle_control_mode; + bool new_data_actuator_armed; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -471,6 +487,28 @@ BlinkM::led() no_data_vehicle_status = 3; } + orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode); + + if (new_data_vehicle_control_mode) { + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode); + no_data_vehicle_control_mode = 0; + } else { + no_data_vehicle_control_mode++; + if(no_data_vehicle_control_mode >= 3) + no_data_vehicle_control_mode = 3; + } + + orb_check(actuator_armed_sub_fd, &new_data_actuator_armed); + + if (new_data_actuator_armed) { + orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed); + no_data_actuator_armed = 0; + } else { + no_data_actuator_armed++; + if(no_data_actuator_armed >= 3) + no_data_actuator_armed = 3; + } + orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); if (new_data_vehicle_gps_position) { @@ -498,7 +536,7 @@ BlinkM::led() /* looking for lipo cells that are connected */ printf("<blinkm> checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; + if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; } printf("<blinkm> cells found:%d\n", num_of_cells); @@ -530,7 +568,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(vehicle_status_raw.flag_system_armed == false) { + if(actuator_armed.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -554,27 +592,24 @@ BlinkM::led() led_color_8 = LED_OFF; 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; + if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { - 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: + //XXX please check + if (vehicle_control_mode.flag_control_position_enabled) led_color_4 = LED_GREEN; - break; + else if (vehicle_control_mode.flag_control_velocity_enabled) + led_color_4 = LED_BLUE; + else if (vehicle_control_mode.flag_control_attitude_enabled) + led_color_4 = LED_YELLOW; + else if (vehicle_control_mode.flag_control_manual_enabled) + led_color_4 = LED_AMBER; + else + led_color_4 = LED_OFF; + } 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; @@ -834,10 +869,10 @@ BlinkM::get_firmware_version(uint8_t version[2]) void blinkm_usage(); void blinkm_usage() { - fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n"); - fprintf(stderr, "options:\n"); - fprintf(stderr, "\t-b --bus i2cbus (3)\n"); - fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n"); + warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}"); + warnx("options:"); + warnx("\t-b --bus i2cbus (3)"); + warnx("\t-a --blinkmaddr blinkmaddr (9)"); } int diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 52a667403..ec9d4ca09 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -118,11 +118,8 @@ ORB_DECLARE(output_pwm); /** start DSM bind */ #define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) -/** stop DSM bind */ -#define DSM_BIND_STOP _IOC(_PWM_SERVO_BASE, 8) - /** Power up DSM receiver */ -#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9) +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index bd027ce0b..3e30e3292 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_armed.h> #include <uORB/topics/actuator_outputs.h> #include <systemlib/err.h> diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 4ad13fc04..1bc3e97a4 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.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_armed.h> #include <uORB/topics/esc_status.h> #include <systemlib/err.h> @@ -135,7 +136,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_armed; unsigned int _motor; int _px4mode; int _frametype; @@ -248,7 +249,7 @@ MK::MK(int bus) : _update_rate(50), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _t_esc_status(0), @@ -513,8 +514,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_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -540,7 +541,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_armed; fds[1].events = POLLIN; log("starting"); @@ -654,7 +655,7 @@ MK::task_main() actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); /* update PWM servo armed status if armed and not locked down */ mk_servo_arm(aa.armed && !aa.lockdown); @@ -700,7 +701,7 @@ MK::task_main() //::close(_t_esc_status); ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_armed); /* make sure servos are off */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a6f337486..6d4019f24 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -70,6 +70,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> +#include <uORB/topics/actuator_armed.h> #include <systemlib/err.h> #ifdef HRT_PPM_CHANNEL @@ -108,7 +109,7 @@ private: unsigned _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_armed; orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; unsigned _num_outputs; @@ -195,7 +196,7 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -424,8 +425,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_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -444,7 +445,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_armed; fds[1].events = POLLIN; #ifdef HRT_PPM_CHANNEL @@ -567,7 +568,7 @@ PX4FMU::task_main() actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); /* update PWM servo armed status if armed and not locked down */ bool set_armed = aa.armed && !aa.lockdown; @@ -603,7 +604,7 @@ PX4FMU::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_armed); /* make sure servos are off */ up_pwm_servo_deinit(); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ae5d6ce19..3aecd9b69 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -74,7 +74,9 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_outputs.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/safety.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/battery_status.h> @@ -128,8 +130,23 @@ public: int set_failsafe_values(const uint16_t *vals, unsigned len); /** - * Print the current status of IO + * 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); + + /** + * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE + */ + int set_idle_values(const uint16_t *vals, unsigned len); + + /** + * Print the current status of IO + */ void print_status(); inline void set_dsm_vcc_ctl(bool enable) @@ -168,8 +185,8 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic - int _t_armed; ///< system armed control topic - int _t_vstatus; ///< system / vehicle status + int _t_actuator_armed; ///< system armed control topic + int _t_vehicle_control_mode; ///< vehicle control mode topic int _t_param; ///< parameter update topic /* advertised topics */ @@ -177,6 +194,7 @@ private: orb_advert_t _to_actuators_effective; ///< effective actuator controls topic orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage + orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; ///< mixed outputs actuator_controls_effective_s _controls_effective; ///< effective controls @@ -195,6 +213,12 @@ private: bool _dsm_vcc_ctl; /** + * System armed + */ + + bool _system_armed; + + /** * Trampoline to the worker task */ static void task_main_trampoline(int argc, char *argv[]); @@ -351,19 +375,21 @@ PX4IO::PX4IO(device::Device *interface) : _status(0), _alarms(0), _t_actuators(-1), - _t_armed(-1), - _t_vstatus(-1), + _t_actuator_armed(-1), + _t_vehicle_control_mode(-1), _t_param(-1), _to_input_rc(0), _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _to_safety(0), _primary_pwm_device(false), _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), _battery_last_timestamp(0), - _dsm_vcc_ctl(false) + _dsm_vcc_ctl(false), + _system_armed(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -454,26 +480,27 @@ PX4IO::init() (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + log("INAIR RESTART RECOVERY (needs commander app running)"); /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO * 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_armed)); /* fill with initial values, clear updated flag */ - vehicle_status_s status; + struct actuator_armed_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_armed), safety_sub, &safety); break; } @@ -499,35 +526,41 @@ 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; /* send command once */ - (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd); /* 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_armed), safety_sub, &safety); } - /* wait 10 ms */ - usleep(10000); + /* wait 50 ms */ + usleep(50000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + if ((hrt_absolute_time() - try_start_time)/1000 > 2000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } - /* keep waiting for state change for 10 s */ - } while (!status.flag_system_armed); + /* re-send if necessary */ + 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 (!safety.armed); /* regular boot, no in-air restart, init IO */ } else { @@ -537,7 +570,8 @@ PX4IO::init() io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); /* publish RC config to IO */ ret = io_set_rc_config(); @@ -580,9 +614,11 @@ void PX4IO::task_main() { hrt_abstime last_poll_time = 0; + int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); log("starting"); + /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. @@ -591,18 +627,18 @@ 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_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ - _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ + _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(_t_vehicle_control_mode, 200); /* 5Hz update rate max. */ _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ if ((_t_actuators < 0) || - (_t_armed < 0) || - (_t_vstatus < 0) || + (_t_actuator_armed < 0) || + (_t_vehicle_control_mode < 0) || (_t_param < 0)) { log("subscription(s) failed"); goto out; @@ -612,9 +648,9 @@ 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_armed; fds[1].events = POLLIN; - fds[2].fd = _t_vstatus; + fds[2].fd = _t_vehicle_control_mode; fds[2].events = POLLIN; fds[3].fd = _t_param; fds[3].events = POLLIN; @@ -690,6 +726,25 @@ PX4IO::task_main() */ if (fds[3].revents & POLLIN) { parameter_update_s pupdate; + int32_t dsm_bind_val; + param_t dsm_bind_param; + + // See if bind parameter has been set, and reset it to 0 + param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); + if (dsm_bind_val) { + if (!_system_armed) { + if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) { + mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x'); + ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7); + } else { + mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected"); + } + } else { + mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); + } + dsm_bind_val = 0; + param_set(dsm_bind_param, &dsm_bind_val); + } /* copy to reset the notification */ orb_copy(ORB_ID(parameter_update), _t_param, &pupdate); @@ -745,30 +800,73 @@ 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::set_idle_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + printf("Sending IDLE values\n"); + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len); +} + + +int PX4IO::io_set_arming_state() { actuator_armed_s armed; ///< system armed state - vehicle_status_s vstatus; ///< overall system state + vehicle_control_mode_s control_mode; ///< vehicle_control_mode - orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); - orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); + orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); uint16_t set = 0; uint16_t clear = 0; + _system_armed = armed.armed; + if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } - if (vstatus.flag_external_manual_override_ok) { + if (control_mode.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; @@ -895,14 +993,14 @@ PX4IO::io_handle_status(uint16_t status) */ /* check for IO reset - force it back to armed if necessary */ - if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED) + if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; - _status &= PX4IO_P_STATUS_FLAGS_ARMED; + _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the sync flag */ @@ -917,6 +1015,27 @@ PX4IO::io_handle_status(uint16_t status) _status = status; } + /** + * Get and handle the safety status + */ + struct safety_s safety; + safety.timestamp = hrt_absolute_time(); + + if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { + safety.safety_off = true; + safety.safety_switch_available = true; + } else { + safety.safety_off = false; + safety.safety_switch_available = true; + } + + /* lazily publish the safety status */ + if (_to_safety > 0) { + orb_publish(ORB_ID(safety), _to_safety, &safety); + } else { + _to_safety = orb_advertise(ORB_ID(safety), &safety); + } + return ret; } @@ -946,7 +1065,7 @@ PX4IO::io_get_status() io_handle_status(regs[0]); io_handle_alarms(regs[1]); - + /* only publish if battery has a valid minimum voltage */ if (regs[2] > 3300) { battery_status_s battery_status; @@ -980,6 +1099,7 @@ PX4IO::io_get_status() _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); } } + return ret; } @@ -1287,7 +1407,8 @@ PX4IO::print_status() uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, - ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), @@ -1358,12 +1479,14 @@ PX4IO::print_status() /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s\n", arming, ((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_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), @@ -1390,7 +1513,10 @@ PX4IO::print_status() } printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("\nidle values"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); printf("\n"); } @@ -1452,16 +1578,11 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); usleep(500000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); - usleep(1000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); - usleep(100000); + usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); - break; - - case DSM_BIND_STOP: - io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); - usleep(500000); break; case DSM_BIND_POWER_UP: @@ -1736,30 +1857,12 @@ bind(int argc, char *argv[]) else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - if (!console) - errx(1, "failed opening console"); - warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); - warnx("Press CTRL-C or 'c' when done."); g_dev->ioctl(nullptr, DSM_BIND_START, pulses); - for (;;) { - usleep(500000L); - /* Check if user wants to quit */ - char c; - if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { - warnx("Done\n"); - g_dev->ioctl(nullptr, DSM_BIND_STOP, 0); - g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); - close(console); - exit(0); - } - } - } + exit(0); + } void @@ -1779,6 +1882,11 @@ test(void) if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) err(1, "failed to get servo count"); + /* tell IO that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (ret != OK) + err(1, "PWM_SERVO_SET_ARM_OK"); + if (ioctl(fd, PWM_SERVO_ARM, 0)) err(1, "failed to arm servos"); @@ -1827,7 +1935,7 @@ test(void) /* Check if user wants to quit */ char c; if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); close(console); exit(0); @@ -2002,6 +2110,111 @@ 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], "idle")) { + + 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 0. */ + uint16_t idle[8]; + + for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) + { + /* set channel to commanline argument or to 0 for non-provided channels */ + if (argc > i + 2) { + idle[i] = atoi(argv[i+2]); + if (idle[i] < 900 || idle[i] > 2100) { + errx(1, "value out of range of 900 < value < 2100. Aborting."); + } + } else { + /* a zero value will the default */ + idle[i] = 0; + } + } + + int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0])); + + if (ret != OK) + errx(ret, "failed setting idle values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { /* @@ -2068,5 +2281,5 @@ px4io_main(int argc, char *argv[]) bind(argc, argv); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'"); } diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 24eec52af..f314b5876 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 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 @@ -237,6 +237,8 @@ private: static const unsigned _default_ntunes; static const uint8_t _note_tab[]; + unsigned _default_tune_number; // number of currently playing default tune (0 for none) + const char *_user_tune; const char *_tune; // current tune string @@ -452,6 +454,11 @@ const char * const ToneAlarm::_default_tunes[] = { "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" "O2E2P64", + "MNT75L1O2G", //arming warning + "MBNT100a8", //battery warning slow + "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" + "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" + "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8", //battery warning fast // XXX why is there a break before a repetition }; const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); @@ -467,6 +474,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : CDev("tone_alarm", "/dev/tone_alarm"), + _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), _next(nullptr) @@ -799,8 +807,12 @@ tune_error: // stop (and potentially restart) the tune tune_end: stop_note(); - if (_repeat) + if (_repeat) { start_tune(_tune); + } else { + _tune = nullptr; + _default_tune_number = 0; + } return; } @@ -869,8 +881,17 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) _tune = nullptr; _next = nullptr; } else { - // play the selected tune - start_tune(_default_tunes[arg - 1]); + /* don't interrupt alarms unless they are repeated */ + if (_tune != nullptr && !_repeat) { + /* abort and let the current tune finish */ + result = -EBUSY; + } else if (_repeat && _default_tune_number == arg) { + /* requested repeating tune already playing */ + } else { + // play the selected tune + _default_tune_number = arg; + start_tune(_default_tunes[arg - 1]); + } } } else { result = -EINVAL; |