diff options
author | Julian Oes <julian@oes.ch> | 2014-05-07 21:11:21 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-05-07 21:11:21 +0200 |
commit | 26f5e550c492fc00341d5a0ae445710325269813 (patch) | |
tree | d4136cbb97dd391ccce638103cd2a7b07ee2231c /src/drivers | |
parent | 470513d9bb67bc19bd0ac70d209c681dc321ddfb (diff) | |
parent | 23937150bce38463612ac170803a06a3424d480d (diff) | |
download | px4-firmware-26f5e550c492fc00341d5a0ae445710325269813.tar.gz px4-firmware-26f5e550c492fc00341d5a0ae445710325269813.tar.bz2 px4-firmware-26f5e550c492fc00341d5a0ae445710325269813.zip |
Merge remote-tracking branch 'px4/ekf_params' into navigator_cleanup_ekf_params
Conflicts:
src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/drv_hrt.h | 1 | ||||
-rw-r--r-- | src/drivers/drv_pwm_output.h | 3 | ||||
-rw-r--r-- | src/drivers/drv_tone_alarm.h | 1 | ||||
-rw-r--r-- | src/drivers/ms5611/ms5611.cpp | 5 | ||||
-rw-r--r-- | src/drivers/px4fmu/fmu.cpp | 254 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 45 | ||||
-rw-r--r-- | src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 |
7 files changed, 210 insertions, 101 deletions
diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index d130d68b3..8bfc90c64 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -41,6 +41,7 @@ #include <sys/types.h> #include <stdbool.h> +#include <inttypes.h> #include <time.h> #include <queue.h> diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 7a93e513e..972573f9f 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -199,6 +199,9 @@ ORB_DECLARE(output_pwm); /** get the lockdown override flag to enable outputs in HIL */ #define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) +/** force safety switch off (to disable use of safety switch) */ +#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) + /* * * diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 147d7123a..b7981e9c4 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -148,6 +148,7 @@ enum { TONE_BATTERY_WARNING_FAST_TUNE, TONE_GPS_WARNING_TUNE, TONE_ARMING_FAILURE_TUNE, + TONE_PARACHUTE_RELEASE_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0ef056273..3fe1b0abc 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -526,6 +526,7 @@ void MS5611::cycle() { int ret; + unsigned dummy; /* collection phase? */ if (_collect_phase) { @@ -542,6 +543,8 @@ MS5611::cycle() } else { //log("collection error %d", ret); } + /* issue a reset command to the sensor */ + _interface->ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ start_cycle(); return; @@ -573,6 +576,8 @@ MS5611::cycle() ret = measure(); if (ret != OK) { //log("measure error %d", ret); + /* issue a reset command to the sensor */ + _interface->ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ start_cycle(); return; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a70ff6c5c..e81902494 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -120,19 +120,25 @@ private: uint32_t _pwm_alt_rate_channels; unsigned _current_update_rate; int _task; - int _t_actuators; - int _t_actuator_armed; - orb_advert_t _t_outputs; + int _armed_sub; + orb_advert_t _outputs_pub; + actuator_armed_s _armed; unsigned _num_outputs; bool _primary_pwm_device; volatile bool _task_should_exit; - bool _armed; + bool _servo_armed; bool _pwm_on; MixerGroup *_mixers; - actuator_controls_s _controls; + uint32_t _groups_required; + uint32_t _groups_subscribed; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num; pwm_limit_t _pwm_limit; uint16_t _failsafe_pwm[_max_actuators]; @@ -149,7 +155,7 @@ private: uint8_t control_group, uint8_t control_index, float &input); - + void subscribe(); int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); @@ -216,15 +222,18 @@ PX4FMU::PX4FMU() : _pwm_alt_rate_channels(0), _current_update_rate(0), _task(-1), - _t_actuators(-1), - _t_actuator_armed(-1), - _t_outputs(0), + _control_subs({-1}), + _poll_fds_num(0), + _armed_sub(-1), + _outputs_pub(-1), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), - _armed(false), + _servo_armed(false), _pwm_on(false), _mixers(nullptr), + _groups_required(0), + _groups_subscribed(0), _failsafe_pwm({0}), _disarmed_pwm({0}), _num_failsafe_set(0), @@ -235,6 +244,14 @@ PX4FMU::PX4FMU() : _max_pwm[i] = PWM_DEFAULT_MAX; } + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); + + memset(_controls, 0, sizeof(_controls)); + memset(_poll_fds, 0, sizeof(_poll_fds)); + _debug_enabled = true; } @@ -448,32 +465,42 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels) } void +PX4FMU::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + warnx("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + if (unsub_groups & (1 << i)) { + warnx("unsubscribe from actuator_controls_%d", i); + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + +void PX4FMU::task_main() { - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* advertise the mixed control outputs */ actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); - - pollfd fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_actuator_armed; - fds[1].events = POLLIN; #ifdef HRT_PPM_CHANNEL // rc input, published to ORB @@ -491,6 +518,12 @@ PX4FMU::task_main() /* loop until killed */ while (!_task_should_exit) { + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + /* force setting update rate */ + _current_update_rate = 0; + } /* * Adjust actuator topic update rate to keep up with @@ -515,7 +548,11 @@ PX4FMU::task_main() } debug("adjusted actuator update interval to %ums", update_rate_in_ms); - orb_set_interval(_t_actuators, update_rate_in_ms); + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); + } + } // set to current max rate, even if we are actually checking slower/faster _current_update_rate = max_rate; @@ -523,7 +560,7 @@ PX4FMU::task_main() /* sleep waiting for data, stopping to check for PPM * input at 100Hz */ - int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS); + int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS); /* this would be bad... */ if (ret < 0) { @@ -537,89 +574,98 @@ PX4FMU::task_main() } else { - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { + /* get controls for required topics */ + unsigned poll_id = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); + } + poll_id++; + } + } - /* get controls - must always do this to avoid spinning */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); + /* can we mix? */ + if (_mixers != nullptr) { - /* can we mix? */ - if (_mixers != nullptr) { + unsigned num_outputs; - unsigned num_outputs; + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; + case MODE_4PWM: + num_outputs = 4; + break; - case MODE_4PWM: - num_outputs = 4; - break; + case MODE_6PWM: + num_outputs = 6; + break; - case MODE_6PWM: - num_outputs = 6; - break; + default: + num_outputs = 0; + break; + } - default: - num_outputs = 0; - break; + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; 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) { + /* + * 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] = -1.0f; } + } - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; 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) { - /* - * 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] = -1.0f; - } - } + uint16_t pwm_limited[num_outputs]; - uint16_t pwm_limited[num_outputs]; + pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + /* output to the servos */ + for (unsigned i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); + } - /* output to the servos */ - for (unsigned i = 0; i < num_outputs; i++) { - up_pwm_servo_set(i, pwm_limited[i]); - } + /* publish mixed control outputs */ + if (_outputs_pub < 0) { + _outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + } else { - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs); } } + } - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - /* update the armed status and check that we're not locked down */ - bool set_armed = aa.armed && !aa.lockdown; + /* update the armed status and check that we're not locked down */ + bool set_armed = _armed.armed && !_armed.lockdown; - if (_armed != set_armed) - _armed = set_armed; + if (_servo_armed != set_armed) + _servo_armed = set_armed; - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (aa.armed || _num_disarmed_set > 0); + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - up_pwm_servo_arm(pwm_on); - } + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); } } @@ -661,8 +707,13 @@ PX4FMU::task_main() } - ::close(_t_actuators); - ::close(_t_actuator_armed); + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs > 0) { + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + } + ::close(_armed_sub); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -684,7 +735,7 @@ PX4FMU::control_callback(uintptr_t handle, { const actuator_controls_s *controls = (actuator_controls_s *)handle; - input = controls->control[control_index]; + input = controls[control_group].control[control_index]; return 0; } @@ -736,6 +787,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET_ARM_OK: case PWM_SERVO_CLEAR_ARM_OK: + case PWM_SERVO_SET_FORCE_SAFETY_OFF: // these are no-ops, as no safety switch break; @@ -1052,6 +1104,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (_mixers != nullptr) { delete _mixers; _mixers = nullptr; + _groups_required = 0; } break; @@ -1060,18 +1113,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) mixer_simple_s *mixinfo = (mixer_simple_s *)arg; SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); + (uintptr_t)_controls, mixinfo); if (mixer->check()) { delete mixer; + _groups_required = 0; ret = -EINVAL; } else { if (_mixers == nullptr) _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); + (uintptr_t)_controls); _mixers->add_mixer(mixer); + _mixers->groups_required(_groups_required); } break; @@ -1082,9 +1137,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); if (_mixers == nullptr) { + _groups_required = 0; ret = -ENOMEM; } else { @@ -1095,7 +1151,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) debug("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; + _groups_required = 0; ret = -EINVAL; + } else { + + _mixers->groups_required(_groups_required); } } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e318e206a..aec6dd3b7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -91,6 +91,8 @@ #include "uploader.h" +#include "modules/dataman/dataman.h" + extern device::Device *PX4IO_i2c_interface() weak_function; extern device::Device *PX4IO_serial_interface() weak_function; @@ -568,9 +570,15 @@ int PX4IO::init() { int ret; + param_t sys_restart_param; + int sys_restart_val = DM_INIT_REASON_VOLATILE; ASSERT(_task == -1); + sys_restart_param = param_find("SYS_RESTART_TYPE"); + /* Indicate restart type is unknown */ + param_set(sys_restart_param, &sys_restart_val); + /* do regular cdev init */ ret = CDev::init(); @@ -675,6 +683,25 @@ PX4IO::init() /* send command to arm system via command API */ vehicle_command_s cmd; + /* send this to itself */ + param_t sys_id_param = param_find("MAV_SYS_ID"); + param_t comp_id_param = param_find("MAV_COMP_ID"); + + int32_t sys_id; + int32_t comp_id; + + if (param_get(sys_id_param, &sys_id)) { + errx(1, "PRM SYSID"); + } + + if (param_get(comp_id_param, &comp_id)) { + errx(1, "PRM CMPID"); + } + + cmd.target_system = sys_id; + cmd.target_component = comp_id; + cmd.source_system = sys_id; + cmd.source_component = comp_id; /* request arming */ cmd.param1 = 1.0f; cmd.param2 = 0; @@ -684,10 +711,7 @@ 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; + /* ask to confirm command */ cmd.confirmation = 1; @@ -720,6 +744,11 @@ PX4IO::init() /* keep waiting for state change for 2 s */ } while (!safety.armed); + /* Indicate restart type is in-flight */ + sys_restart_val = DM_INIT_REASON_IN_FLIGHT; + param_set(sys_restart_param, &sys_restart_val); + + /* regular boot, no in-air restart, init IO */ } else { @@ -745,6 +774,10 @@ PX4IO::init() } } + /* Indicate restart type is power on */ + sys_restart_val = DM_INIT_REASON_POWER_ON; + param_set(sys_restart_param, &sys_restart_val); + } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ @@ -2129,6 +2162,10 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) case PWM_SERVO_GET_DISABLE_LOCKDOWN: *(unsigned *)arg = _lockdown_override; + + case PWM_SERVO_SET_FORCE_SAFETY_OFF: + /* force safety swith off */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); break; case DSM_BIND_START: diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 8ed0de58c..810f4aacc 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -335,6 +335,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP"; + _default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag"; // parachute release _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone @@ -346,6 +347,7 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast _tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning _tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure"; //fail to arm + _tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release"; // parachute release } ToneAlarm::~ToneAlarm() |