diff options
31 files changed, 1756 insertions, 779 deletions
diff --git a/ROMFS/Makefile b/ROMFS/Makefile index d827fa491..15167bf47 100644 --- a/ROMFS/Makefile +++ b/ROMFS/Makefile @@ -81,7 +81,7 @@ all: $(ROMFS_HEADER) $(ROMFS_HEADER): $(ROMFS_IMG) $(dir $(ROMFS_HEADER)) @echo Generating the ROMFS header... - @(cd $(dir $(ROMFS_IMG)) && xxd -i $(notdir $(ROMFS_IMG))) > $@ + @(cd $(dir $(ROMFS_IMG)) && xxd -i $(notdir $(ROMFS_IMG))) | sed -e 's/char/const char/' > $@ $(ROMFS_IMG): $(ROMFS_WORKDIR) @echo Generating the ROMFS image... diff --git a/ROMFS/mixers/FMU_Q.mix b/ROMFS/mixers/FMU_Q.mix index a35d299fd..cf64ef568 100644 --- a/ROMFS/mixers/FMU_Q.mix +++ b/ROMFS/mixers/FMU_Q.mix @@ -25,13 +25,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 -5000 -8000 0 -10000 10000 -S: 0 1 -8000 -8000 0 -10000 10000 +S: 0 0 5000 8000 0 -10000 10000 +S: 0 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 -8000 -5000 0 -10000 10000 -S: 0 1 8000 8000 0 -10000 10000 +S: 0 0 8000 5000 0 -10000 10000 +S: 0 1 -8000 -8000 0 -10000 10000 Output 2 -------- diff --git a/ROMFS/mixers/FMU_X5.mix b/ROMFS/mixers/FMU_X5.mix index 981466704..9f81e1dc3 100644 --- a/ROMFS/mixers/FMU_X5.mix +++ b/ROMFS/mixers/FMU_X5.mix @@ -23,13 +23,13 @@ for the elevons. M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 3000 5000 0 -10000 10000 -S: 0 1 5000 5000 0 -10000 10000 +S: 0 0 -3000 -5000 0 -10000 10000 +S: 0 1 -5000 -5000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 0 0 5000 3000 0 -10000 10000 -S: 0 1 -5000 -5000 0 -10000 10000 +S: 0 0 -5000 -3000 0 -10000 10000 +S: 0 1 5000 5000 0 -10000 10000 Output 2 -------- diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 7ca77e513..bd972f03f 100755 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -72,38 +72,6 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); -static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - -static float dt = 0.005f; -/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ -static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ -static float x_aposteriori_k[12]; /**< states */ -static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, - }; /**< init: diagonal matrix with big values */ - -static float x_aposteriori[12]; -static float P_aposteriori[144]; - -/* output euler angles */ -static float euler[3] = {0.0f, 0.0f, 0.0f}; - -static float Rot_matrix[9] = {1.f, 0, 0, - 0, 1.f, 0, - 0, 0, 1.f - }; /**< init: identity matrix */ - - static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ @@ -153,7 +121,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 12000, + 12400, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); @@ -193,6 +161,38 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) */ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) { + +const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds + + float dt = 0.005f; +/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ + float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ + float x_aposteriori_k[12]; /**< states */ + float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, + }; /**< init: diagonal matrix with big values */ + + float x_aposteriori[12]; + float P_aposteriori[144]; + + /* output euler angles */ + float euler[3] = {0.0f, 0.0f, 0.0f}; + + float Rot_matrix[9] = {1.f, 0, 0, + 0, 1.f, 0, + 0, 0, 1.f + }; /**< init: identity matrix */ + // print text printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); fflush(stdout); diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 68ab2c5b6..7c0a25f86 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -243,11 +243,11 @@ static int led_off(int led) } enum AUDIO_PATTERN { - AUDIO_PATTERN_ERROR = 1, - AUDIO_PATTERN_NOTIFY_POSITIVE = 2, - AUDIO_PATTERN_NOTIFY_NEUTRAL = 3, - AUDIO_PATTERN_NOTIFY_NEGATIVE = 4, - AUDIO_PATTERN_TETRIS = 5 + AUDIO_PATTERN_ERROR = 2, + AUDIO_PATTERN_NOTIFY_POSITIVE = 3, + AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, + AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, + AUDIO_PATTERN_NOTIFY_CHARGE = 6 }; int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) diff --git a/apps/controllib/block/BlockParam.cpp b/apps/controllib/block/BlockParam.cpp index b3f49f7db..fd12e365d 100644 --- a/apps/controllib/block/BlockParam.cpp +++ b/apps/controllib/block/BlockParam.cpp @@ -46,7 +46,7 @@ namespace control { -BlockParamBase::BlockParamBase(Block *parent, const char *name) : +BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_prefix) : _handle(PARAM_INVALID) { char fullname[blockNameLengthMax]; @@ -61,8 +61,10 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name) : if (!strcmp(name, "")) { strncpy(fullname, parentName, blockNameLengthMax); - } else { + } else if (parent_prefix) { snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name); + } else { + strncpy(fullname, name, blockNameLengthMax); } parent->getParams().add(this); diff --git a/apps/controllib/block/BlockParam.hpp b/apps/controllib/block/BlockParam.hpp index 7f86d1717..58a9bfc0d 100644 --- a/apps/controllib/block/BlockParam.hpp +++ b/apps/controllib/block/BlockParam.hpp @@ -53,7 +53,12 @@ namespace control class __EXPORT BlockParamBase : public ListNode<BlockParamBase *> { public: - BlockParamBase(Block *parent, const char *name); + /** + * Instantiate a block param base. + * + * @param parent_prefix Set to true to include the parent name in the parameter name + */ + BlockParamBase(Block *parent, const char *name, bool parent_prefix=true); virtual ~BlockParamBase() {}; virtual void update() = 0; const char *getName() { return param_name(_handle); } @@ -68,8 +73,8 @@ template<class T> class __EXPORT BlockParam : public BlockParamBase { public: - BlockParam(Block *block, const char *name) : - BlockParamBase(block, name), + BlockParam(Block *block, const char *name, bool parent_prefix=true) : + BlockParamBase(block, name, parent_prefix), _val() { update(); } diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index b84a54fee..7f5711c47 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -171,10 +171,10 @@ BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent, _headingHold(this, ""), _velocityHold(this, ""), _altitudeHold(this, ""), - _trimAil(this, "TRIM_AIL"), - _trimElv(this, "TRIM_ELV"), - _trimRdr(this, "TRIM_RDR"), - _trimThr(this, "TRIM_THR") + _trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */ + _trimElv(this, "TRIM_PITCH", false), /* general pitch trim */ + _trimRdr(this, "TRIM_YAW", false), /* general yaw trim */ + _trimThr(this, "TRIM_THR", true) /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */ { } @@ -323,7 +323,7 @@ void BlockMultiModeBacksideAutopilot::update() _att.rollspeed, _att.pitchspeed, _att.yawspeed ); _actuators.control[CH_AIL] = _backsideAutopilot.getAileron(); - _actuators.control[CH_ELV] = - _backsideAutopilot.getElevator(); + _actuators.control[CH_ELV] = _backsideAutopilot.getElevator(); _actuators.control[CH_RDR] = _backsideAutopilot.getRudder(); _actuators.control[CH_THR] = _backsideAutopilot.getThrottle(); @@ -355,7 +355,7 @@ void BlockMultiModeBacksideAutopilot::update() _att.rollspeed, _att.pitchspeed, _att.yawspeed); _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = - _stabilization.getElevator(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); _actuators.control[CH_RDR] = _stabilization.getRudder(); _actuators.control[CH_THR] = _manual.throttle; } diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 54c7d4266..56265995f 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -135,7 +135,7 @@ public: virtual int setMode(int mode); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - static const char *script_names[]; + static const char *const script_names[]; private: enum ScriptID { @@ -219,7 +219,7 @@ namespace } /* list of script names, must match script ID numbers */ -const char *BlinkM::script_names[] = { +const char *const BlinkM::script_names[] = { "USER", "RGB", "WHITE_FLASH", @@ -499,7 +499,7 @@ BlinkM::led() 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; } - printf("<blinkm> cells found:%u\n", num_of_cells); + printf("<blinkm> cells found:%d\n", num_of_cells); } else { if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { @@ -827,7 +827,7 @@ BlinkM::get_firmware_version(uint8_t version[2]) { const uint8_t msg = 'Z'; - return transfer(&msg, sizeof(msg), version, sizeof(version)); + return transfer(&msg, sizeof(msg), version, 2); } void blinkm_usage() { diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index f3f753ebe..07831f20c 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -36,7 +36,7 @@ * * Servo values can be set with the PWM_SERVO_SET ioctl, by writing a * pwm_output_values structure to the device, or by publishing to the - * output_pwm ObjDev. + * output_pwm ORB topic. * Writing a value of 0 to a channel suppresses any output for that * channel. */ @@ -60,7 +60,7 @@ __BEGIN_DECLS #define PWM_OUTPUT_DEVICE_PATH "/dev/pwm_output" /** - * Maximum number of PWM output channels in the system. + * Maximum number of PWM output channels supported by the device. */ #define PWM_OUTPUT_MAX_CHANNELS 16 @@ -82,14 +82,14 @@ struct pwm_output_values { }; /* - * ObjDev tag for PWM outputs. + * ORB tag for PWM outputs. */ ORB_DECLARE(output_pwm); /* * ioctl() definitions * - * Note that ioctls and ObjDev updates should not be mixed, as the + * Note that ioctls and ORB updates should not be mixed, as the * behaviour of the system in this case is not defined. */ #define _PWM_SERVO_BASE 0x2a00 @@ -100,20 +100,14 @@ ORB_DECLARE(output_pwm); /** disarm all servo outputs (stop generating pulses) */ #define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) -/** set update rate in Hz */ -#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) +/** set alternate servo update rate */ +#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2) /** get the number of servos in *(unsigned *)arg */ #define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3) -/** set debug level for servo IO */ -#define PWM_SERVO_SET_DEBUG _IOC(_PWM_SERVO_BASE, 4) - -/** enable in-air restart */ -#define PWM_SERVO_INAIR_RESTART_ENABLE _IOC(_PWM_SERVO_BASE, 5) - -/** disable in-air restart */ -#define PWM_SERVO_INAIR_RESTART_DISABLE _IOC(_PWM_SERVO_BASE, 6) +/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */ +#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4) /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) @@ -121,6 +115,10 @@ ORB_DECLARE(output_pwm); /** get a single specific servo value */ #define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo) +/** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels + * whose update rates must be the same. + */ +#define PWM_SERVO_GET_RATEGROUP(_n) _IOC(_PWM_SERVO_BASE, 0x60 + _n) /* * Low-level PWM output interface. @@ -157,7 +155,7 @@ __EXPORT extern void up_pwm_servo_deinit(void); __EXPORT extern void up_pwm_servo_arm(bool armed); /** - * Set the servo update rate + * Set the servo update rate for all rate groups. * * @param rate The update rate in Hz to set. * @return OK on success, -ERANGE if an unsupported update rate is set. @@ -165,6 +163,25 @@ __EXPORT extern void up_pwm_servo_arm(bool armed); __EXPORT extern int up_pwm_servo_set_rate(unsigned rate); /** + * Get a bitmap of output channels assigned to a given rate group. + * + * @param group The rate group to query. Rate groups are assigned contiguously + * starting from zero. + * @return A bitmap of channels assigned to the rate group, or zero if + * the group number has no channels. + */ +__EXPORT extern uint32_t up_pwm_servo_get_rate_group(unsigned group); + +/** + * Set the update rate for a given rate group. + * + * @param group The rate group whose update rate will be changed. + * @param rate The update rate in Hz. + * @return OK if the group was adjusted, -ERANGE if an unsupported update rate is set. + */ +__EXPORT extern int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate); + +/** * Set the current output value for a channel. * * @param channel The channel to set. diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index 861ed7924..d9aa772d4 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -158,6 +158,7 @@ HIL::HIL() : CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/), _mode(MODE_NONE), _update_rate(50), + _current_update_rate(0), _task(-1), _t_actuators(-1), _t_armed(-1), @@ -511,9 +512,14 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_UPDATE_RATE: + // HIL always outputs at the alternate (usually faster) rate g_hil->set_pwm_rate(arg); break; + case PWM_SERVO_SELECT_UPDATE_RATE: + // HIL always outputs at the alternate (usually faster) rate + break; + case PWM_SERVO_SET(2): case PWM_SERVO_SET(3): if (_mode != MODE_4PWM) { @@ -549,6 +555,14 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } + case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { + // no restrictions on output grouping + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + + *(uint32_t *)arg = (1 << channel); + break; + } + case MIXERIOCGETOUTPUTCOUNT: if (_mode == MODE_4PWM) { *(unsigned *)arg = 4; @@ -641,7 +655,7 @@ enum PortMode { PortMode g_port_mode; int -hil_new_mode(PortMode new_mode, int update_rate) +hil_new_mode(PortMode new_mode) { // uint32_t gpio_bits; @@ -699,8 +713,6 @@ hil_new_mode(PortMode new_mode, int update_rate) /* (re)set the PWM output mode */ g_hil->set_mode(servo_mode); - if ((servo_mode != HIL::MODE_NONE) && (update_rate != 0)) - g_hil->set_pwm_rate(update_rate); return OK; } @@ -786,59 +798,34 @@ int hil_main(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNDEFINED; - int pwm_update_rate_in_hz = 0; - - if (!strcmp(argv[1], "test")) - test(); - - if (!strcmp(argv[1], "fake")) - fake(argc - 1, argv + 1); + const char *verb = argv[1]; if (hil_start() != OK) - errx(1, "failed to start the FMU driver"); + errx(1, "failed to start the HIL driver"); /* * Mode switches. - * - * XXX use getopt? */ - for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */ - if (!strcmp(argv[i], "mode_pwm")) { - new_mode = PORT1_FULL_PWM; + // this was all cut-and-pasted from the FMU driver; it's junk + if (!strcmp(verb, "mode_pwm")) { + new_mode = PORT1_FULL_PWM; - } else if (!strcmp(argv[i], "mode_pwm_serial")) { - new_mode = PORT1_PWM_AND_SERIAL; + } else if (!strcmp(verb, "mode_pwm_serial")) { + new_mode = PORT1_PWM_AND_SERIAL; - } else if (!strcmp(argv[i], "mode_pwm_gpio")) { - new_mode = PORT1_PWM_AND_GPIO; - - } else if (!strcmp(argv[i], "mode_port2_pwm8")) { - new_mode = PORT2_8PWM; + } else if (!strcmp(verb, "mode_pwm_gpio")) { + new_mode = PORT1_PWM_AND_GPIO; - } else if (!strcmp(argv[i], "mode_port2_pwm12")) { - new_mode = PORT2_8PWM; + } else if (!strcmp(verb, "mode_port2_pwm8")) { + new_mode = PORT2_8PWM; - } else if (!strcmp(argv[i], "mode_port2_pwm16")) { - new_mode = PORT2_8PWM; - } + } else if (!strcmp(verb, "mode_port2_pwm12")) { + new_mode = PORT2_8PWM; - /* look for the optional pwm update rate for the supported modes */ - if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { - // if (new_mode == PORT1_FULL_PWM || new_mode == PORT1_PWM_AND_GPIO) { - // XXX all modes have PWM settings - if (argc > i + 1) { - pwm_update_rate_in_hz = atoi(argv[i + 1]); - printf("pwm update rate: %d Hz\n", pwm_update_rate_in_hz); - } else { - fprintf(stderr, "missing argument for pwm update rate (-u)\n"); - return 1; - } - // } else { - // fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n"); - // } - } - } + } else if (!strcmp(verb, "mode_port2_pwm16")) { + new_mode = PORT2_8PWM; + } /* was a new mode set? */ if (new_mode != PORT_MODE_UNDEFINED) { @@ -848,12 +835,17 @@ hil_main(int argc, char *argv[]) return OK; /* switch modes */ - return hil_new_mode(new_mode, pwm_update_rate_in_hz); + return hil_new_mode(new_mode); } - /* test, etc. here */ + if (!strcmp(verb, "test")) + test(); + + if (!strcmp(verb, "fake")) + fake(argc - 1, argv + 1); + fprintf(stderr, "HIL: unrecognized command, try:\n"); - fprintf(stderr, " mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); + fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n"); return -EINVAL; } diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 08420822a..59ab5936e 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -595,7 +595,7 @@ MS5611::cycle() * spam the console with the message. */ } else { - log("collection error %d", ret); + //log("collection error %d", ret); } /* reset the collection state machine and try again */ start_cycle(); @@ -627,7 +627,7 @@ MS5611::cycle() /* measurement phase */ ret = measure(); if (ret != OK) { - log("measure error %d", ret); + //log("measure error %d", ret); /* reset the collection state machine and try again */ start_cycle(); return; diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 8e13f7c62..e54724536 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -90,14 +90,18 @@ public: virtual int init(); int set_mode(Mode mode); - int set_pwm_rate(unsigned rate); + + int set_pwm_alt_rate(unsigned rate); + int set_pwm_alt_channels(uint32_t channels); private: static const unsigned _max_actuators = 4; Mode _mode; - int _update_rate; - int _current_update_rate; + unsigned _pwm_default_rate; + unsigned _pwm_alt_rate; + uint32_t _pwm_alt_rate_channels; + unsigned _current_update_rate; int _task; int _t_actuators; int _t_armed; @@ -121,6 +125,7 @@ private: uint8_t control_index, float &input); + int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); struct GPIOConfig { @@ -163,7 +168,10 @@ PX4FMU *g_fmu; PX4FMU::PX4FMU() : CDev("fmuservo", "/dev/px4fmu"), _mode(MODE_NONE), - _update_rate(50), + _pwm_default_rate(50), + _pwm_alt_rate(50), + _pwm_alt_rate_channels(0), + _current_update_rate(0), _task(-1), _t_actuators(-1), _t_armed(-1), @@ -263,27 +271,31 @@ PX4FMU::set_mode(Mode mode) * are presented on the output pins. */ switch (mode) { - case MODE_2PWM: - debug("MODE_2PWM"); - /* multi-port with flow control lines as PWM */ - /* XXX magic numbers */ - up_pwm_servo_init(0x3); - _update_rate = 50; /* default output rate */ - break; + case MODE_2PWM: // multi-port with flow control lines as PWM + case MODE_4PWM: // multi-port as 4 PWM outs + debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4); + + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; - case MODE_4PWM: - debug("MODE_4PWM"); - /* multi-port as 4 PWM outs */ /* XXX magic numbers */ - up_pwm_servo_init(0xf); - _update_rate = 50; /* default output rate */ + up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + break; case MODE_NONE: debug("MODE_NONE"); - /* disable servo outputs and set a very low update rate */ + + _pwm_default_rate = 10; /* artificially reduced output rate */ + _pwm_alt_rate = 10; + _pwm_alt_rate_channels = 0; + + /* disable servo outputs - no need to set rates */ up_pwm_servo_deinit(); - _update_rate = 10; + break; default: @@ -295,15 +307,63 @@ PX4FMU::set_mode(Mode mode) } int -PX4FMU::set_pwm_rate(unsigned rate) +PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate) { - if ((rate > 500) || (rate < 10)) - return -EINVAL; + debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate); + + for (unsigned pass = 0; pass < 2; pass++) { + for (unsigned group = 0; group < _max_actuators; group++) { + + // get the channel mask for this rate group + uint32_t mask = up_pwm_servo_get_rate_group(group); + if (mask == 0) + continue; + + // all channels in the group must be either default or alt-rate + uint32_t alt = rate_map & mask; + + if (pass == 0) { + // preflight + if ((alt != 0) && (alt != mask)) { + warn("rate group %u mask %x bad overlap %x", group, mask, alt); + // not a legal map, bail + return -EINVAL; + } + } else { + // set it - errors here are unexpected + if (alt != 0) { + if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) { + warn("rate group set alt failed"); + return -EINVAL; + } + } else { + if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { + warn("rate group set default failed"); + return -EINVAL; + } + } + } + } + } + _pwm_alt_rate_channels = rate_map; + _pwm_default_rate = default_rate; + _pwm_alt_rate = alt_rate; - _update_rate = rate; return OK; } +int +PX4FMU::set_pwm_alt_rate(unsigned rate) +{ + return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate); +} + +int +PX4FMU::set_pwm_alt_channels(uint32_t channels) +{ + return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate); +} + void PX4FMU::task_main() { @@ -353,24 +413,31 @@ PX4FMU::task_main() /* loop until killed */ while (!_task_should_exit) { - /* handle update rate changes */ - if (_current_update_rate != _update_rate) { - int update_rate_in_ms = int(1000 / _update_rate); + /* + * Adjust actuator topic update rate to keep up with + * the highest servo update rate configured. + * + * We always mix at max rate; some channels may update slower. + */ + unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate; + if (_current_update_rate != max_rate) { + _current_update_rate = max_rate; + int update_rate_in_ms = int(1000 / _current_update_rate); /* 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; + /* reject slower than 10 Hz updates */ + if (update_rate_in_ms > 100) { + update_rate_in_ms = 100; } + debug("adjusted actuator update interval to %ums", update_rate_in_ms); orb_set_interval(_t_actuators, update_rate_in_ms); - up_pwm_servo_set_rate(_update_rate); - _current_update_rate = _update_rate; + + // set to current max rate, even if we are actually checking slower/faster + _current_update_rate = max_rate; } /* sleep waiting for data, stopping to check for PPM @@ -527,7 +594,6 @@ int PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; - int channel; lock(); @@ -541,7 +607,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; case PWM_SERVO_SET_UPDATE_RATE: - set_pwm_rate(arg); + ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); + break; + + case PWM_SERVO_SELECT_UPDATE_RATE: + ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate); break; case PWM_SERVO_SET(2): @@ -555,9 +625,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_SET(0): case PWM_SERVO_SET(1): if (arg < 2100) { - channel = cmd - PWM_SERVO_SET(0); - up_pwm_servo_set(channel, arg); - + up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); } else { ret = -EINVAL; } @@ -573,12 +641,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) /* FALLTHROUGH */ case PWM_SERVO_GET(0): - case PWM_SERVO_GET(1): { - channel = cmd - PWM_SERVO_GET(0); - *(servo_position_t *)arg = up_pwm_servo_get(channel); - break; - } + case PWM_SERVO_GET(1): + *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); + break; + + case PWM_SERVO_GET_RATEGROUP(0): + case PWM_SERVO_GET_RATEGROUP(1): + case PWM_SERVO_GET_RATEGROUP(2): + case PWM_SERVO_GET_RATEGROUP(3): + *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); + break; + case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: if (_mode == MODE_4PWM) { *(unsigned *)arg = 4; @@ -812,7 +886,7 @@ enum PortMode { PortMode g_port_mode; int -fmu_new_mode(PortMode new_mode, int update_rate) +fmu_new_mode(PortMode new_mode) { uint32_t gpio_bits; PX4FMU::Mode servo_mode; @@ -864,9 +938,6 @@ fmu_new_mode(PortMode new_mode, int update_rate) /* (re)set the PWM output mode */ g_fmu->set_mode(servo_mode); - if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0)) - g_fmu->set_pwm_rate(update_rate); - return OK; } @@ -962,57 +1033,31 @@ int fmu_main(int argc, char *argv[]) { PortMode new_mode = PORT_MODE_UNSET; - int pwm_update_rate_in_hz = 0; - - if (!strcmp(argv[1], "test")) - test(); - - if (!strcmp(argv[1], "fake")) - fake(argc - 1, argv + 1); + const char *verb = argv[1]; if (fmu_start() != OK) errx(1, "failed to start the FMU driver"); /* * Mode switches. - * - * XXX use getopt? */ - for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */ - if (!strcmp(argv[i], "mode_gpio")) { - new_mode = PORT_FULL_GPIO; + if (!strcmp(verb, "mode_gpio")) { + new_mode = PORT_FULL_GPIO; - } else if (!strcmp(argv[i], "mode_serial")) { - new_mode = PORT_FULL_SERIAL; + } else if (!strcmp(verb, "mode_serial")) { + new_mode = PORT_FULL_SERIAL; - } else if (!strcmp(argv[i], "mode_pwm")) { - new_mode = PORT_FULL_PWM; + } else if (!strcmp(verb, "mode_pwm")) { + new_mode = PORT_FULL_PWM; - } else if (!strcmp(argv[i], "mode_gpio_serial")) { - new_mode = PORT_GPIO_AND_SERIAL; + } else if (!strcmp(verb, "mode_gpio_serial")) { + new_mode = PORT_GPIO_AND_SERIAL; - } else if (!strcmp(argv[i], "mode_pwm_serial")) { - new_mode = PORT_PWM_AND_SERIAL; - - } else if (!strcmp(argv[i], "mode_pwm_gpio")) { - new_mode = PORT_PWM_AND_GPIO; - } - - /* look for the optional pwm update rate for the supported modes */ - if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) { - if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) { - if (argc > i + 1) { - pwm_update_rate_in_hz = atoi(argv[i + 1]); - - } else { - errx(1, "missing argument for pwm update rate (-u)"); - return 1; - } + } else if (!strcmp(verb, "mode_pwm_serial")) { + new_mode = PORT_PWM_AND_SERIAL; - } else { - errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio"); - } - } + } else if (!strcmp(verb, "mode_pwm_gpio")) { + new_mode = PORT_PWM_AND_GPIO; } /* was a new mode set? */ @@ -1023,12 +1068,17 @@ fmu_main(int argc, char *argv[]) return OK; /* switch modes */ - return fmu_new_mode(new_mode, pwm_update_rate_in_hz); + int ret = fmu_new_mode(new_mode); + exit(ret == OK ? 0 : 1); } - /* test, etc. here */ + if (!strcmp(verb, "test")) + test(); + + if (!strcmp(verb, "fake")) + fake(argc - 1, argv + 1); fprintf(stderr, "FMU: unrecognised command, try:\n"); - fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); + fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n"); exit(1); } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 27c885ed7..4f6938a14 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -86,6 +86,8 @@ #include "uploader.h" #include <debug.h> +#define PX4IO_SET_DEBUG _IOC(0xff00, 0) +#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) class PX4IO : public device::I2C { @@ -98,6 +100,17 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); + /** + * Set the update rate for actuator outputs from FMU to IO. + * + * @param rate The rate in Hz actuator outpus are sent to IO. + * Min 10 Hz, max 400 Hz + */ + int set_update_rate(int rate); + + /** + * Print the current status of IO + */ void print_status(); private: @@ -1223,14 +1236,15 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); - printf("alarms 0x%04x%s%s%s%s%s%s\n", + printf("alarms 0x%04x%s%s%s%s%s%s%s\n", alarms, ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), - ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : "")); + ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) ? " PWM_ERROR" : "")); printf("vbatt %u ibatt %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT)); @@ -1269,10 +1283,10 @@ PX4IO::print_status() ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); - printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n", + 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_LOWRATE), - io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); printf("vbatt scale %u ibatt scale %u ibatt bias %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE), @@ -1320,36 +1334,39 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0); break; - case PWM_SERVO_INAIR_RESTART_ENABLE: - /* set the 'in-air restart' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); + case PWM_SERVO_SET_UPDATE_RATE: + /* set the requested alternate rate */ + if ((arg >= 50) && (arg <= 400)) { /* TODO: we could go higher for e.g. TurboPWM */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); + } else { + ret = -EINVAL; + } break; - case PWM_SERVO_INAIR_RESTART_DISABLE: - /* unset the 'in-air restart' bit */ - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); - break; + case PWM_SERVO_SELECT_UPDATE_RATE: { - case PWM_SERVO_SET_UPDATE_RATE: - /* set the requested rate */ - if ((arg >= 50) && (arg <= 400)) { - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE, arg); - } else { + /* blindly clear the PWM update alarm - might be set for some other reason */ + io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); + + /* attempt to set the rate map */ + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES, arg); + + /* check that the changes took */ + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + if (alarms & PX4IO_P_STATUS_ALARMS_PWM_ERROR) { ret = -EINVAL; + io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR); } break; + } case PWM_SERVO_GET_COUNT: *(unsigned *)arg = _max_actuators; break; - case PWM_SERVO_SET_DEBUG: - /* set the debug level */ - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); - break; - - case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): { + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { + /* TODO: we could go lower for e.g. TurboPWM */ unsigned channel = cmd - PWM_SERVO_SET(0); if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) { ret = -EINVAL; @@ -1361,7 +1378,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS): { + case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS - 1): { unsigned channel = cmd - PWM_SERVO_GET(0); @@ -1379,6 +1396,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } + case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { + + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + + uint32_t value = io_reg_get(PX4IO_PAGE_PWM_INFO, PX4IO_RATE_MAP_BASE + channel); + + *(uint32_t *)arg = value; + break; + } + case GPIO_RESET: ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0); break; @@ -1443,6 +1470,20 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } + case PX4IO_SET_DEBUG: + /* set the debug level */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg); + break; + + case PX4IO_INAIR_RESTART_ENABLE: + /* set/clear the 'in-air restart' bit */ + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); + } + break; + default: /* not a recognized value */ ret = -ENOTTY; @@ -1466,12 +1507,51 @@ PX4IO::write(file *filp, const char *buffer, size_t len) return count * 2; } +int +PX4IO::set_update_rate(int rate) +{ + int interval_ms = 1000 / rate; + if (interval_ms < 5) { + interval_ms = 5; + warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); + } + + if (interval_ms > 100) { + interval_ms = 100; + warnx("update rate too low, limiting to %d ms (%d Hz).", interval_ms, 1000 / interval_ms); + } + + _update_interval = interval_ms; + return 0; +} + + extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace { void +start(int argc, char *argv[]) +{ + if (g_dev != nullptr) + errx(1, "already loaded"); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + + if (OK != g_dev->init()) { + delete g_dev; + errx(1, "driver init failed"); + } + + exit(0); +} + +void test(void) { int fd; @@ -1565,32 +1645,20 @@ px4io_main(int argc, char *argv[]) if (argc < 2) goto out; - if (!strcmp(argv[1], "start")) { - - if (g_dev != nullptr) - errx(1, "already loaded"); - - /* create the driver - it will set g_dev */ - (void)new PX4IO(); + if (!strcmp(argv[1], "start")) + start(argc - 1, argv + 1); - if (g_dev == nullptr) - errx(1, "driver alloc failed"); + if (!strcmp(argv[1], "limit")) { - if (OK != g_dev->init()) { - delete g_dev; - errx(1, "driver init failed"); - } + if (g_dev != nullptr) { - /* look for the optional pwm update rate for the supported modes */ - if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) { - if (argc > 2 + 1) { -#warning implement this + if ((argc > 2)) { + g_dev->set_update_rate(atoi(argv[2])); } else { - fprintf(stderr, "missing argument for pwm update rate (-u)\n"); + errx(1, "missing argument (50 - 200 Hz)"); return 1; } } - exit(0); } @@ -1602,7 +1670,7 @@ px4io_main(int argc, char *argv[]) * We can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ - g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0); + g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); } else { errx(1, "not loaded"); } @@ -1623,16 +1691,16 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { - if (g_dev != nullptr) { - printf("[px4io] loaded\n"); - g_dev->print_status(); - } else { - printf("[px4io] not loaded\n"); - } - - exit(0); + if (g_dev != nullptr) { + printf("[px4io] loaded\n"); + g_dev->print_status(); + } else { + printf("[px4io] not loaded\n"); } + exit(0); + } + if (!strcmp(argv[1], "debug")) { if (argc <= 2) { printf("usage: px4io debug LEVEL\n"); @@ -1646,7 +1714,7 @@ px4io_main(int argc, char *argv[]) /* we can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ - int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level); + int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level); if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); exit(1); @@ -1721,5 +1789,5 @@ px4io_main(int argc, char *argv[]) monitor(); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit' or 'update'"); } diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index 2b8641f00..d7316e1f7 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -68,10 +68,6 @@ #include <stm32_gpio.h> #include <stm32_tim.h> - -/* default rate (in Hz) of PWM updates */ -static uint32_t pwm_update_rate = 50; - #define REG(_tmr, _reg) (*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg)) #define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) @@ -93,6 +89,10 @@ static uint32_t pwm_update_rate = 50; #define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) #define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) +static void pwm_timer_init(unsigned timer); +static void pwm_timer_set_rate(unsigned timer, unsigned rate); +static void pwm_channel_init(unsigned channel); + static void pwm_timer_init(unsigned timer) { @@ -113,11 +113,8 @@ pwm_timer_init(unsigned timer) /* configure the timer to free-run at 1MHz */ rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1; - /* and update at the desired rate */ - rARR(timer) = (1000000 / pwm_update_rate) - 1; - - /* generate an update event; reloads the counter and all registers */ - rEGR(timer) = GTIM_EGR_UG; + /* default to updating at 50Hz */ + pwm_timer_set_rate(timer, 50); /* note that the timer is left disabled - arming is performed separately */ } @@ -272,19 +269,41 @@ up_pwm_servo_deinit(void) } int -up_pwm_servo_set_rate(unsigned rate) +up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate) { - if ((rate < 50) || (rate > 400)) + /* limit update rate to 1..10000Hz; somewhat arbitrary but safe */ + if (rate < 1) + return -ERANGE; + if (rate > 10000) return -ERANGE; - for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { - if (pwm_timers[i].base != 0) - pwm_timer_set_rate(i, rate); - } + if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0)) + return ERROR; + + pwm_timer_set_rate(group, rate); return OK; } +int +up_pwm_servo_set_rate(unsigned rate) +{ + for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) + up_pwm_servo_set_rate_group_update(i, rate); +} + +uint32_t +up_pwm_servo_get_rate_group(unsigned group) +{ + unsigned channels = 0; + + for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { + if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group)) + channels |= (1 << i); + } + return channels; +} + void up_pwm_servo_arm(bool armed) { diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp index 6799a0a21..baa652d8a 100644 --- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -35,27 +35,59 @@ * Driver for the PX4 audio alarm port, /dev/tone_alarm. * * The tone_alarm driver supports a set of predefined "alarm" - * patterns and one user-supplied pattern. Patterns are ordered by - * priority, with a higher-priority pattern interrupting any - * lower-priority pattern that might be playing. + * tunes and one user-supplied tune. * * The TONE_SET_ALARM ioctl can be used to select a predefined - * alarm pattern, from 1 - <TBD>. Selecting pattern zero silences + * alarm tune, from 1 - <TBD>. Selecting tune zero silences * the alarm. * - * To supply a custom pattern, write an array of 1 - <TBD> tone_note - * structures to /dev/tone_alarm. The custom pattern has a priority - * of zero. + * Tunes follow the syntax of the Microsoft GWBasic/QBasic PLAY + * statement, with some exceptions and extensions. + * + * From Wikibooks: + * + * PLAY "[string expression]" + * + * Used to play notes and a score ... The tones are indicated by letters A through G. + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * immediately after the note letter. See this example: + * + * PLAY "C C# C C#" + * + * Whitespaces are ignored inside the string expression. There are also codes that + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators + * that change the properties are effective for the notes following that indicator. + * + * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration + * amount but rather a note type; L1 - whole note, L2 - half note, L4 - quarter note, etc. + * (L8, L16, L32, L64, ...). By default, n = 4. + * For triplets and quintets, use L3, L6, L12, ... and L5, L10, L20, ... series respectively. + * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" + * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. + * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. + * Remember that C- is equivalent to B. + * < > Changes the current octave respectively down or up one level. + * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) + * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. + * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. + * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. + * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. + * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. + * It can be used for a pause as well. + * + * Extensions/variations: + * + * MB MF The MF command causes the tune to play once and then stop. The MB command causes the + * tune to repeat when it ends. * - * Patterns will normally play once and then silence (if a pattern - * was overridden it will not resume). A pattern may be made to - * repeat by inserting a note with the duration set to - * DURATION_REPEAT. This pattern will loop until either a - * higher-priority pattern is started or pattern zero is requested - * via the ioctl. */ #include <nuttx/config.h> +#include <debug.h> #include <drivers/device/device.h> #include <drivers/drv_tone_alarm.h> @@ -69,6 +101,8 @@ #include <errno.h> #include <stdio.h> #include <unistd.h> +#include <math.h> +#include <ctype.h> #include <arch/board/board.h> #include <drivers/drv_hrt.h> @@ -202,141 +236,233 @@ public: virtual ssize_t write(file *filp, const char *buffer, size_t len); private: - static const unsigned _max_pattern = 6; - static const unsigned _pattern_none = _max_pattern + 1; - static const unsigned _pattern_user = 0; - static const unsigned _max_pattern_len = 40; - static const unsigned _note_max = TONE_NOTE_MAX; - - static const tone_note _patterns[_max_pattern][_max_pattern_len]; - static const uint16_t _notes[_note_max]; - - unsigned _current_pattern; - unsigned _next_note; - - hrt_call _note_end; - tone_note _user_pattern[_max_pattern_len]; - + static const unsigned _tune_max = 1024; // be reasonable about user tunes + static const char * const _default_tunes[]; + static const unsigned _default_ntunes; + static const uint8_t _note_tab[]; + + const char *_user_tune; + + const char *_tune; // current tune string + const char *_next; // next note in the string + + unsigned _tempo; + unsigned _note_length; + enum { MODE_NORMAL, MODE_LEGATO, MODE_STACCATO} _note_mode; + unsigned _octave; + unsigned _silence_length; // if nonzero, silence before next note + bool _repeat; // if true, tune restarts at end + + hrt_call _note_call; // HRT callout for note completion + + // Convert a note value in the range C1 to B7 into a divisor for + // the configured timer's clock. + // + unsigned note_to_divisor(unsigned note); + + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of + // dots following in the play string. + // + unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); + + // Calculate the duration in microseconds of a rest corresponding to + // a given note length. + // + unsigned rest_duration(unsigned rest_length, unsigned dots); + + // Start playing the note + // + void start_note(unsigned note); + + // Stop playing the current note and make the player 'safe' + // + void stop_note(); + + // Start playing the tune + // + void start_tune(const char *tune); + + // Parse the next note out of the string and play it + // + void next_note(); + + // Find the next character in the string, discard any whitespace and + // return the canonical (uppercase) version. + // + int next_char(); + + // Extract a number from the string, consuming all the digit characters. + // + unsigned next_number(); + + // Consume dot characters from the string, returning the number consumed. + // + unsigned next_dots(); + + // hrt_call trampoline for next_note + // static void next_trampoline(void *arg); - void next(); - bool check(tone_note *pattern); - void stop(); -}; - -/* predefined patterns for alarms 1-_max_pattern */ -const tone_note ToneAlarm::_patterns[_max_pattern][_max_pattern_len] = { - { - {TONE_NOTE_A7, 12}, - {TONE_NOTE_D8, 12}, - {TONE_NOTE_C8, 12}, - {TONE_NOTE_A7, 12}, - {TONE_NOTE_D8, 12}, - {TONE_NOTE_C8, 12}, - {TONE_NOTE_D8, 4}, - {TONE_NOTE_C8, 4}, - {TONE_NOTE_D8, 4}, - {TONE_NOTE_C8, 4}, - {TONE_NOTE_D8, 4}, - {TONE_NOTE_C8, 4}, - }, - {{TONE_NOTE_B6, 100}, {TONE_NOTE_B6, DURATION_REPEAT}}, - {{TONE_NOTE_C7, 100}}, - {{TONE_NOTE_D7, 100}}, - {{TONE_NOTE_E7, 100}}, - { - //This is tetris ;) - {TONE_NOTE_C6, 40}, - {TONE_NOTE_G5, 20}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_A5S, 40}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_G5, 20}, - {TONE_NOTE_F5, 40}, - {TONE_NOTE_F5, 20}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_C6, 40}, - {TONE_NOTE_A5S, 20}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_G5, 60}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_A5S, 40}, - {TONE_NOTE_C6, 40}, - {TONE_NOTE_G5S, 40}, - {TONE_NOTE_F5, 40}, - {TONE_NOTE_F5, 60}, - {TONE_NOTE_A5S, 40}, - {TONE_NOTE_C6S, 20}, - {TONE_NOTE_F6, 40}, - {TONE_NOTE_D6S, 20}, - {TONE_NOTE_C6S, 20}, - {TONE_NOTE_C6, 60}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_C6, 40}, - {TONE_NOTE_A5S, 20}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_G5, 40}, - {TONE_NOTE_G5, 20}, - {TONE_NOTE_G5S, 20}, - {TONE_NOTE_A5S, 40}, - {TONE_NOTE_C6, 40}, - {TONE_NOTE_G5S, 40}, - {TONE_NOTE_F5, 40}, - {TONE_NOTE_F5, 60}, - } }; -const uint16_t ToneAlarm::_notes[_note_max] = { - 63707, /* E4 */ - 60132, /* F4 */ - 56758, /* F#4/Gb4 */ - 53571, /* G4 */ - 50565, /* G#4/Ab4 */ - 47727, /* A4 */ - 45048, /* A#4/Bb4 */ - 42520, /* B4 */ - 40133, /* C5 */ - 37880, /* C#5/Db5 */ - 35755, /* D5 */ - 33748, /* D#5/Eb5 */ - 31853, /* E5 */ - 30066, /* F5 */ - 28378, /* F#5/Gb5 */ - 26786, /* G5 */ - 25282, /* G#5/Ab5 */ - 23863, /* A5 */ - 22524, /* A#5/Bb5 */ - 21260, /* B5 */ - 20066, /* C6 */ - 18940, /* C#6/Db6 */ - 17877, /* D6 */ - 16874, /* D#6/Eb6 */ - 15927, /* E6 */ - 15033, /* F6 */ - 14189, /* F#6/Gb6 */ - 13393, /* G6 */ - 12641, /* G#6/Ab6 */ - 11931, /* A6 */ - 11262, /* A#6/Bb6 */ - 10630, /* B6 */ - 10033, /* C7 */ - 9470, /* C#7/Db7 */ - 8938, /* D7 */ - 8437, /* D#7/Eb7 */ - 7963, /* E7 */ - 7516, /* F7 */ - 7094, /* F#7/Gb7 */ - 6696, /* G7 */ - 6320, /* G#7/Ab7 */ - 5965, /* A7 */ - 5631, /* A#7/Bb7 */ - 5315, /* B7 */ - 5016, /* C8 */ - 4735, /* C#8/Db8 */ - 4469, /* D8 */ - 4218 /* D#8/Eb8 */ +// predefined tune array +const char * const ToneAlarm::_default_tunes[] = { + "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc", // startup tune + "MBT200a8a8a8PaaaP", // ERROR tone + "MFT200e8a8a", // NotifyPositive tone + "MFT200e8e", // NotifyNeutral tone + "MFT200e8c8e8c8e8c8", // NotifyNegative tone + "MFT90O3C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C16.C32C16.C32C16.C32G16.E32G16.E32G16.E32C4", // charge! + "MFT60O3C32O2A32F16F16F32G32A32A+32O3C16C16C16O2A16", // dixie + "MFT90O2C16C16C16F8.A8C16C16C16F8.A4P16P8", // cucuracha + "MNT150L8O2GGABGBADGGABL4GL8F+", // yankee + "MFT200O3C4.O2A4.G4.F4.D8E8F8D4F8C2.O2G4.O3C4.O2A4.F4.D8E8F8G4A8G2P8", // daisy + "T200O2B4P8B16B16B4P8B16B16B8G+8E8G+8B8G+8B8O3E8" // william tell + "O2B8G+8E8G+8B8G+8B8O3E8O2B4P8B16B16B4P8B16" + "O2B16B4P8B16B16B4P8B16B16B8B16B16B8B8B8B16" + "O2B16B8B8B8B16B16B8B8B8B16B16B8B8B2B2B8P8" + "P4P4P8O1B16B16B8B16B16B8B16B16O2E8F+8G+8" + "O1B16B16B8B16B16O2E8G+16G+16F+8D+8O1B8B16" + "O1B16B8B16B16B8B16B16O2E8F+8G+8E16G+16B4" + "O2B16A16G+16F+16E8G+8E8O3B16B16B8B16B16B8" + "O3B16B16O4E8F+8G+8O3B16B16B8B16B16O4E8G+16" + "O4G+16F+8D+8O3B8B16B16B8B16B16B8B16B16O4E8" + "O4F+8G+8E16G+16B4B16A16G+16F+16E8G+8E8O3G+16" + "O3G+16G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8" + "O4C+8O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8" + "O3G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8O2B16" + "O2B16B8F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8F+8F+8O3F+16F+16F+8F+16F+16F+8" + "O3G+8A8F+4A8G+8E4G+8F+8O2B16B16B8O1B16B16" + "O1B8B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16" + "O1B16O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16" + "O1B8B16B16O2E8F+8G+8E16G+16B4B16A16G+16F+16" + "O2E8G+8E8O3B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8" + "O3B16B16B8B16B16B8B16B16O4E8F+8G+8E16G+16" + "O4B4B16A16G+16F+16E8G+8E8O3E64F64G64A64B64" + "O4C64D64E8E16E16E8E8G+4.F+8E8D+8E8C+8O3B16" + "O4C+16O3B16O4C+16O3B16O4C+16D+16E16O3A16" + "O3B16A16B16A16B16O4C+16D+16O3G+16A16G+16" + "O3A16G+16A16B16O4C+16O3F+16G+16F+16G+16F+16" + "O3G+16F+16G+16F+16G+16F+16D+16O2B16O3B16" + "O4C+16D+16E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16A16F+16E16E8P8C+4C+16O2C16" + "O3C+16O2C16O3D+16C+16O2B16A16A16G+16E16C+16" + "O2C+16C+16C+16C+16E16D+16O1C16G+16G+16G+16" + "O1G+16G+16G+16O2C+16E16G+16O3C+16C+16C+16" + "O3C+16C+16O2C16O3C+16O2C16O3D+16C+16O2B16" + "O2A16A16G+16E16C+16C+16C+16C+16C+16E16D+16" + "O1C16G+16G+16G+16G+16G+16G+16O2C+16E16G+16" + "O3C+16E16D+16C+16D+16O2C16G+16G+16G+16O3G+16" + "O3E16C+16D+16O2C16G+16G+16G+16O3G+16E16C+16" + "O3D+16O2B16G+16G+16A+16G16D+16D+16G+16G16" + "O2G+16G16G+16A16G+16F+16E16O1B16A+16B16O2E16" + "O1B16O2F+16O1B16O2G+16E16D+16E16G+16E16A16" + "O2F+16B16O3G+16F+16E16D+16F+16E16C+16O2B16" + "O3C+16O2B16O3C+16D+16E16F+16G+16O2A16B16" + "O2A16B16O3C+16D+16E16F+16O2G+16A16G+16A16" + "O2C16O3C+16D+16E16O2F+16G+16F+16G+16F+16" + "O2G+16F+16G+16F+16G+16F+16D+16O1B16C16O2C+16" + "O2D+16E16O1B16A+16B16O2E16O1B16O2F+16O1B16" + "O2G+16E16D+16E16G+16E16A16F+16B16O3G+16F+16" + "O3E16D+16F+16E16C+16O2B16O3C+16O2B16O3C+16" + "O3D+16E16F+16G+16O2A16B16A16B16O3C+16D+16" + "O3E16F+16O2G+16A16G+16A16B16O3C+16D+16E16" + "O2F+16O3C+16O2C16O3C+16D+16C+16O2A16F+16" + "O2E16O3E16F+16G+16A16B16O4C+16D+16E8E16E16" + "O4E8E8G+4.F8E8D+8E8C+8O3B16O4C+16O3B16O4C+16" + "O3B16O4C+16D+16E16O3A16B16A16B16A16B16O4C+16" + "O4D+16O3G+16A16G+16A16G+16A16B16O4C+16O3F+16" + "O3G+16F+16G+16F+16G+16F+16G+16F+16G+16F+16" + "O3D+16O2B16O3B16O4C+16D+16E8E16E16E8E8G+4." + "O4F+8E8D+8E8C+8O3B16O4C+16O3B16O4C+16O3B16" + "O4C+16D+16E16O3A16B16A16B16A16B16O4C+16D+16" + "O3G+16A16G+16A16G+16A16B16O4C+16O3F+16G+16" + "O3F+16G+16F+16A16G+16F+16E8O2B8O3E8G+16G+16" + "O3G+8G+16G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8" + "O3G+8O4C+8O3G+8F+8E8D+8C+8G+16G+16G+8G+16" + "O3G+16G+8G+16G+16G+8O4C+8O3G+8O4C+8O3G+8" + "O4C+8O3B8A+8B8A+8B8G+16G+16G+8G+16G+16G+8" + "O3G+16G+16G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3G+8" + "O3F+8E8D+8C+8G+16G+16G+8G+16G+16G+8G+16G+16" + "O3G+8O4C+8O3G+8O4C+8O3G+8O4C+8O3B8A+8B8A+8" + "O3B8O2F+16F+16F+8F+16F+16F+8G+8A8F+4A8G+8" + "O2E4G+8F+8B8O1B8O2F+16F+16F+8F+16F+16F+8" + "O2G+8A8F+4A8G+8E4G+8F+8B16B16B8O1B16B16B8" + "O1B16B16B8B16B16O2E8F+8G+8O1B16B16B8B16B16" + "O2E8G+16G+16F+8D+8O1B8B16B16B8B16B16B8B16" + "O1B16O2E8F+8G+8E16G+16B4B16A16G+16F+16E8" + "O1B8O2E8O3B16B16B8B16B16B8B16B16O4E8F+8G+8" + "O3B16B16B8B16B16O4E8G+16G+16F+8D+8O3B8B16" + "O3B16B8B16B16B8B16B16O4E8F+8G+8O3E16G+16" + "O3B4B16A16G+16F+16E16F+16G+16A16G+16A16B16" + "O4C+16O3B16O4C+16D+16E16D+16E16F+16G+16A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16E16F+16" + "O3G+16A16G+16A16B16O4C+16O3B16O4C+16D+16" + "O4E16D+16E16F+16G+16A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16O4A16O3B16O4A16O3B16O4A16" + "O3B16O4A16O3B16P16G+16O4G+16O3G+16P16D+16" + "O4D+16O3D+16P16E16O4E16O3E16P16A16O4A16O3A16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G16O3G16O4G8E8C8E8" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+8E8O3B8O4E8G+16O3G+16O4G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16" + "O3A+16O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16" + "O4A+8G8E8G8B8P16A+16P16A16P16G+16P16F+16" + "P16O4E16P16D+16P16C+16P16O3B16P16A+16P16" + "O3A16P16G+16P16F+16P16E16P16D+16P16F+16E16" + "O3F+16G+16A16G+16A16B16O4C+16O3B16O4C+16" + "O4D+16E16D+16E16F+16G+16A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16E16F+16G+16A16G+16A16" + "O3B16O4C+16O3B16O4C+16D+16E16D+16E16F+16" + "O4G+16A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "O4A16O3B16O4A16O3B16O4A16O3B16O4A16O3B16" + "P16O3G+16O4G+16O3G+16P16D+16O4D+16O3D+16" + "P16O3E16O4E16O3E16P16A16O4A16O3A16P16G+16" + "O4G+16O3G+16P16D+16O4D+16O3D+16P16E16O4E16" + "O3E16P16A16O4A16O3A16O4G16O3G16O4G16O3G16" + "O4G16O3G16O4G16O3G16O4G8E8C8E8G+16O3G+16" + "O4G+16O3G+16O4G+16O3G+16O4G+16O3G+16O4G+8" + "O4E8O3B8O4E8G+16O3G+16O4G+16O3G+16O4G+16" + "O3G+16O4G+16O3G+16O4G+8F8C+8F8A+16O3A+16" + "O4A+16O3A+16O4A+16O3A+16O4A+16O3A+16O4A+8" + "O4G8E8G8B8P16A+16P16A16P16G+16P16F+16P16" + "O4E16P16D+16P16C+16P16O3B16P16A+16P16A16" + "P16O3G+16P16F+16P16E16P16D+16P16F16E16D+16" + "O3E16D+16E8B16B16B8B16B16B8B16B16O4E8F+8" + "O4G+8O3B16B16B8B16B16B8B16B16O4G+8A8B8P8" + "O4E8F+8G+8P8O3G+8A8B8P8P2O2B16C16O3C+16D16" + "O3D+16E16F16F+16G16G+16A16A+16B16C16O4C+16" + "O4D+16E16D+16F+16D+16E16D+16F+16D+16E16D+16" + "O4F+16D+16E16D+16F+16D+16E16D+16F+16D+16" + "O4E16D+16F+16D+16E16D+16F+16D+16E16D+16F+16" + "O4D+16E8E16O3E16O4E16O3E16O4E16O3E16O4E8" + "O3B16O2B16O3B16O2B16O3B16O2B16O3B8G+16O2G+16" + "O3G+16O2G+16O3G+16O2G+16O3G8E16O2E16O3E16" + "O2E16O3E16O2E16O3E8E16E16E8E8E8O2B16B16B8" + "O2B8B8G+16G+16G+8G+8G+8E16E16E8E8E8O1B8O2E8" + "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" + "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" + "O2E2P64", }; +const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); + +// semitone offsets from C for the characters 'A'-'G' +const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; + /* * Driver 'main' command. */ @@ -345,8 +471,9 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : CDev("tone_alarm", "/dev/tone_alarm"), - _current_pattern(_pattern_none), - _next_note(0) + _user_tune(nullptr), + _tune(nullptr), + _next(nullptr) { // enable debug() calls //_debug_enabled = true; @@ -386,13 +513,8 @@ ToneAlarm::init() /* toggle the CC output each time the count passes 1 */ TONE_rCCR = 1; - /* - * Configure the timebase to free-run at half max frequency. - * XXX this should be more flexible in order to get a better - * frequency range, but for the F4 with the APB1 timers based - * at 42MHz, this gets us down to ~320Hz or so. - */ - rPSC = 1; + /* default the timer to a prescale value of 1; playing notes will change this */ + rPSC = 0; /* make sure the timer is running */ rCR1 = GTIM_CR1_CEN; @@ -401,196 +523,413 @@ ToneAlarm::init() return OK; } -int -ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) +unsigned +ToneAlarm::note_to_divisor(unsigned note) { - int result = 0; - unsigned new_pattern = arg; + // compute the frequency first (Hz) + float freq = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f); - debug("ioctl %i %u", cmd, new_pattern); + float period = 0.5f / freq; -// irqstate_t flags = irqsave(); + // and the divisor, rounded to the nearest integer + unsigned divisor = (period * TONE_ALARM_CLOCK) + 0.5f; - /* decide whether to increase the alarm level to cmd or leave it alone */ - switch (cmd) { - case TONE_SET_ALARM: - debug("TONE_SET_ALARM %u", arg); - - if (new_pattern == 0) { - /* cancel any current alarm */ - _current_pattern = _pattern_none; - next(); - - } else if (new_pattern > _max_pattern) { - - /* not a legal alarm value */ - result = -ERANGE; - - } else if ((_current_pattern == _pattern_none) || (new_pattern > _current_pattern)) { - - /* higher priority than the current alarm */ - _current_pattern = new_pattern; - _next_note = 0; + return divisor; +} - /* and start playing it */ - next(); +unsigned +ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots) +{ + unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - } else { - /* current pattern is higher priority than the new pattern, ignore */ - } + if (note_length == 0) + note_length = 1; + unsigned note_period = whole_note_period / note_length; + switch (_note_mode) { + case MODE_NORMAL: + silence = note_period / 8; + break; + case MODE_STACCATO: + silence = note_period / 4; break; - default: - result = -ENOTTY; + case MODE_LEGATO: + silence = 0; break; } + note_period -= silence; -// irqrestore(flags); - - /* give it to the superclass if we didn't like it */ - if (result == -ENOTTY) - result = CDev::ioctl(filp, cmd, arg); + unsigned dot_extension = note_period / 2; + while (dots--) { + note_period += dot_extension; + dot_extension /= 2; + } - return result; + return note_period; } -int -ToneAlarm::write(file *filp, const char *buffer, size_t len) +unsigned +ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) { - irqstate_t flags; - - /* sanity-check the size of the write */ - if (len > (_max_pattern_len * sizeof(tone_note))) - return -EFBIG; + unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if ((len % sizeof(tone_note)) || (len == 0)) - return -EIO; + if (rest_length == 0) + rest_length = 1; - if (!check((tone_note *)buffer)) - return -EIO; + unsigned rest_period = whole_note_period / rest_length; - flags = irqsave(); + unsigned dot_extension = rest_period / 2; + while (dots--) { + rest_period += dot_extension; + dot_extension /= 2; + } - /* if we aren't playing an alarm tone */ - if (_current_pattern <= _pattern_user) { + return rest_period; +} - /* reset the tone state to play the new user pattern */ - _current_pattern = _pattern_user; - _next_note = 0; +void +ToneAlarm::start_note(unsigned note) +{ + // compute the divisor + unsigned divisor = note_to_divisor(note); - /* copy in the new pattern */ - memset(_user_pattern, 0, sizeof(_user_pattern)); - memcpy(_user_pattern, buffer, len); + // pick the lowest prescaler value that we can use + // (note that the effective prescale value is 1 greater) + unsigned prescale = divisor / 65536; - /* and start it */ - debug("starting user pattern"); - next(); - } + // calculate the timer period for the selected prescaler value + unsigned period = (divisor / (prescale + 1)) - 1; - irqrestore(flags); + rPSC = prescale; // load new prescaler + rARR = period; // load new toggle period + rEGR = GTIM_EGR_UG; // force a reload of the period + rCCER |= TONE_CCER; // enable the output - return len; } void -ToneAlarm::next_trampoline(void *arg) +ToneAlarm::stop_note() { - ToneAlarm *ta = (ToneAlarm *)arg; + /* stop the current note */ + rCCER &= ~TONE_CCER; - ta->next(); + /* + * Make sure the GPIO is not driving the speaker. + * + * XXX this presumes PX4FMU and the onboard speaker driver FET. + */ + stm32_gpiowrite(GPIO_TONE_ALARM, 0); } void -ToneAlarm::next(void) +ToneAlarm::start_tune(const char *tune) { - const tone_note *np; + // kill any current playback + hrt_cancel(&_note_call); + + // record the tune + _tune = tune; + _next = tune; + + // initialise player state + _tempo = 120; + _note_length = 4; + _note_mode = MODE_NORMAL; + _octave = 4; + _silence_length = 0; + _repeat = false; // otherwise command-line tunes repeat forever... + + // schedule a callback to start playing + hrt_call_after(&_note_call, 0, (hrt_callout)next_trampoline, this); +} - /* stop the current note */ - rCCER &= ~TONE_CCER; +void +ToneAlarm::next_note() +{ + // do we have an inter-note gap to wait for? + if (_silence_length > 0) { + stop_note(); + hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this); + _silence_length = 0; + return; + } - /* if we are no longer playing a pattern, we have nothing else to do here */ - if (_current_pattern == _pattern_none) { - stop(); + // make sure we still have a tune - may be removed by the write / ioctl handler + if ((_next == nullptr) || (_tune == nullptr)) { + stop_note(); return; } - ASSERT(_next_note < _note_max); + // parse characters out of the string until we have resolved a note + unsigned note = 0; + unsigned note_length = _note_length; + unsigned duration; + + while (note == 0) { + // we always need at least one character from the string + int c = next_char(); + if (c == 0) + goto tune_end; + _next++; + + switch (c) { + case 'L': // select note length + _note_length = next_number(); + if (_note_length < 1) + goto tune_error; + break; + + case 'O': // select octave + _octave = next_number(); + if (_octave > 6) + _octave = 6; + break; - /* find the note to play */ - if (_current_pattern == _pattern_user) { - np = &_user_pattern[_next_note]; + case '<': // decrease octave + if (_octave > 0) + _octave--; + break; - } else { - np = &_patterns[_current_pattern - 1][_next_note]; - } + case '>': // increase octave + if (_octave < 6) + _octave++; + break; + + case 'M': // select inter-note gap + c = next_char(); + if (c == 0) + goto tune_error; + _next++; + switch (c) { + case 'N': + _note_mode = MODE_NORMAL; + break; + case 'L': + _note_mode = MODE_LEGATO; + break; + case 'S': + _note_mode = MODE_STACCATO; + break; + case 'F': + _repeat = false; + break; + case 'B': + _repeat = true; + break; + default: + goto tune_error; + } + break; - /* work out which note is next */ - _next_note++; + case 'P': // pause for a note length + stop_note(); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); + return; + + case 'T': { // change tempo + unsigned nt = next_number(); + + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + } else { + goto tune_error; + } + break; + } - if (_next_note >= _note_max) { - /* hit the end of the pattern, stop */ - _current_pattern = _pattern_none; + case 'N': // play an arbitrary note + note = next_number(); + if (note > 84) + goto tune_error; + if (note == 0) { + // this is a rest - pause for the current note length + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; + } + break; - } else if (np[1].duration == DURATION_END) { - /* hit the end of the pattern, stop */ - _current_pattern = _pattern_none; + case 'A'...'G': // play a note in the current octave + note = _note_tab[c - 'A'] + (_octave * 12) + 1; + c = next_char(); + switch (c) { + case '#': // up a semitone + case '+': + if (note < 84) + note++; + _next++; + break; + case '-': // down a semitone + if (note > 1) + note--; + _next++; + break; + default: + // 0 / no next char here is OK + break; + } + // shorthand length notation + note_length = next_number(); + if (note_length == 0) + note_length = _note_length; + break; - } else if (np[1].duration == DURATION_REPEAT) { - /* next note is a repeat, rewind in preparation */ - _next_note = 0; + default: + goto tune_error; + } } - /* set the timer to play the note, if required */ - if (np->pitch <= TONE_NOTE_SILENCE) { + // compute the duration of the note and the following silence (if any) + duration = note_duration(_silence_length, note_length, next_dots()); + + // start playing the note + start_note(note); + + // and arrange a callback when the note should stop + hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this); + return; - /* set reload based on the pitch */ - rARR = _notes[np->pitch]; + // tune looks bad (unexpected EOF, bad character, etc.) +tune_error: + lowsyslog("tune error\n"); + _repeat = false; // don't loop on error - /* force an update, reloads the counter and all registers */ - rEGR = GTIM_EGR_UG; + // stop (and potentially restart) the tune +tune_end: + stop_note(); + if (_repeat) + start_tune(_tune); + return; +} - /* enable the output */ - rCCER |= TONE_CCER; +int +ToneAlarm::next_char() +{ + while (isspace(*_next)) { + _next++; } + return toupper(*_next); +} - /* arrange a callback when the note/rest is done */ - hrt_call_after(&_note_end, (hrt_abstime)10000 * np->duration, (hrt_callout)next_trampoline, this); +unsigned +ToneAlarm::next_number() +{ + unsigned number = 0; + int c; + + for (;;) { + c = next_char(); + if (!isdigit(c)) + return number; + _next++; + number = (number * 10) + (c - '0'); + } } -bool -ToneAlarm::check(tone_note *pattern) +unsigned +ToneAlarm::next_dots() { - for (unsigned i = 0; i < _note_max; i++) { + unsigned dots = 0; - /* first note must not be repeat or end */ - if ((i == 0) && - ((pattern[i].duration == DURATION_END) || (pattern[i].duration == DURATION_REPEAT))) - return false; + while (next_char() == '.') { + _next++; + dots++; + } + return dots; +} - if (pattern[i].duration == DURATION_END) - break; +void +ToneAlarm::next_trampoline(void *arg) +{ + ToneAlarm *ta = (ToneAlarm *)arg; - /* pitch must be legal */ - if (pattern[i].pitch >= _note_max) - return false; + ta->next_note(); +} + + +int +ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) +{ + int result = OK; + + debug("ioctl %i %u", cmd, arg); + +// irqstate_t flags = irqsave(); + + /* decide whether to increase the alarm level to cmd or leave it alone */ + switch (cmd) { + case TONE_SET_ALARM: + debug("TONE_SET_ALARM %u", arg); + + if (arg <= _default_ntunes) { + if (arg == 0) { + // stop the tune + _tune = nullptr; + _next = nullptr; + } else { + // play the selected tune + start_tune(_default_tunes[arg - 1]); + } + } else { + result = -EINVAL; + } + + break; + + default: + result = -ENOTTY; + break; } - return true; +// irqrestore(flags); + + /* give it to the superclass if we didn't like it */ + if (result == -ENOTTY) + result = CDev::ioctl(filp, cmd, arg); + + return result; } -void -ToneAlarm::stop() +int +ToneAlarm::write(file *filp, const char *buffer, size_t len) { - /* stop the current note */ - rCCER &= ~TONE_CCER; + // sanity-check the buffer for length and nul-termination + if (len > _tune_max) + return -EFBIG; - /* - * Make sure the GPIO is not driving the speaker. - * - * XXX this presumes PX4FMU and the onboard speaker driver FET. - */ - stm32_gpiowrite(GPIO_TONE_ALARM, 0); + // if we have an existing user tune, free it + if (_user_tune != nullptr) { + + // if we are playing the user tune, stop + if (_tune == _user_tune) { + _tune = nullptr; + _next = nullptr; + } + + // free the old user tune + free((void *)_user_tune); + _user_tune = nullptr; + } + + // if the new tune is empty, we're done + if (buffer[0] == '\0') + return OK; + + // allocate a copy of the new tune + _user_tune = strndup(buffer, len); + if (_user_tune == nullptr) + return -ENOMEM; + + // and play it + start_tune(_user_tune); + + return len; } /** @@ -602,7 +941,7 @@ namespace ToneAlarm *g_dev; int -play_pattern(unsigned pattern) +play_tune(unsigned tune) { int fd, ret; @@ -611,7 +950,8 @@ play_pattern(unsigned pattern) if (fd < 0) err(1, "/dev/tone_alarm"); - ret = ioctl(fd, TONE_SET_ALARM, pattern); + ret = ioctl(fd, TONE_SET_ALARM, tune); + close(fd); if (ret != 0) err(1, "TONE_SET_ALARM"); @@ -619,12 +959,30 @@ play_pattern(unsigned pattern) exit(0); } +int +play_string(const char *str) +{ + int fd, ret; + + fd = open("/dev/tone_alarm", O_WRONLY); + + if (fd < 0) + err(1, "/dev/tone_alarm"); + + ret = write(fd, str, strlen(str) + 1); + close(fd); + + if (ret < 0) + err(1, "play tune"); + exit(0); +} + } // namespace int tone_alarm_main(int argc, char *argv[]) { - unsigned pattern; + unsigned tune; /* start the driver lazily */ if (g_dev == nullptr) { @@ -640,13 +998,21 @@ tone_alarm_main(int argc, char *argv[]) } if ((argc > 1) && !strcmp(argv[1], "start")) - play_pattern(1); + play_tune(1); if ((argc > 1) && !strcmp(argv[1], "stop")) - play_pattern(0); + play_tune(0); + + if ((tune = strtol(argv[1], nullptr, 10)) != 0) + play_tune(tune); - if ((pattern = strtol(argv[1], nullptr, 10)) != 0) - play_pattern(pattern); + /* if it looks like a PLAY string... */ + if (strlen(argv[1]) > 2) { + const char *str = argv[1]; + if (str[0] == 'M') { + play_string(str); + } + } errx(1, "unrecognised command, try 'start', 'stop' or an alarm number"); } diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 8cefc298f..83e01e63c 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -56,8 +56,4 @@ PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity -// trim -PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1) -PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1) -PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1) PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 7c34fb474..1dd3fc2d8 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -91,32 +91,32 @@ static uint64_t last_sensor_timestamp; static void *uorb_receive_thread(void *arg); struct listener { - void (*callback)(struct listener *l); + void (*callback)(const struct listener *l); int *subp; uintptr_t arg; }; -static void l_sensor_combined(struct listener *l); -static void l_vehicle_attitude(struct listener *l); -static void l_vehicle_gps_position(struct listener *l); -static void l_vehicle_status(struct listener *l); -static void l_rc_channels(struct listener *l); -static void l_input_rc(struct listener *l); -static void l_global_position(struct listener *l); -static void l_local_position(struct listener *l); -static void l_global_position_setpoint(struct listener *l); -static void l_local_position_setpoint(struct listener *l); -static void l_attitude_setpoint(struct listener *l); -static void l_actuator_outputs(struct listener *l); -static void l_actuator_armed(struct listener *l); -static void l_manual_control_setpoint(struct listener *l); -static void l_vehicle_attitude_controls(struct listener *l); -static void l_debug_key_value(struct listener *l); -static void l_optical_flow(struct listener *l); -static void l_vehicle_rates_setpoint(struct listener *l); -static void l_home(struct listener *l); - -struct listener listeners[] = { +static void l_sensor_combined(const struct listener *l); +static void l_vehicle_attitude(const struct listener *l); +static void l_vehicle_gps_position(const struct listener *l); +static void l_vehicle_status(const struct listener *l); +static void l_rc_channels(const struct listener *l); +static void l_input_rc(const struct listener *l); +static void l_global_position(const struct listener *l); +static void l_local_position(const struct listener *l); +static void l_global_position_setpoint(const struct listener *l); +static void l_local_position_setpoint(const struct listener *l); +static void l_attitude_setpoint(const struct listener *l); +static void l_actuator_outputs(const struct listener *l); +static void l_actuator_armed(const struct listener *l); +static void l_manual_control_setpoint(const struct listener *l); +static void l_vehicle_attitude_controls(const struct listener *l); +static void l_debug_key_value(const struct listener *l); +static void l_optical_flow(const struct listener *l); +static void l_vehicle_rates_setpoint(const struct listener *l); +static void l_home(const struct listener *l); + +static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, {l_vehicle_attitude, &mavlink_subs.att_sub, 0}, {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, @@ -144,7 +144,7 @@ struct listener listeners[] = { static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); void -l_sensor_combined(struct listener *l) +l_sensor_combined(const struct listener *l) { struct sensor_combined_s raw; @@ -199,7 +199,7 @@ l_sensor_combined(struct listener *l) } void -l_vehicle_attitude(struct listener *l) +l_vehicle_attitude(const struct listener *l) { struct vehicle_attitude_s att; @@ -222,7 +222,7 @@ l_vehicle_attitude(struct listener *l) } void -l_vehicle_gps_position(struct listener *l) +l_vehicle_gps_position(const struct listener *l) { struct vehicle_gps_position_s gps; @@ -256,7 +256,7 @@ l_vehicle_gps_position(struct listener *l) } void -l_vehicle_status(struct listener *l) +l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); @@ -280,7 +280,7 @@ l_vehicle_status(struct listener *l) } void -l_rc_channels(struct listener *l) +l_rc_channels(const struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); @@ -288,7 +288,7 @@ l_rc_channels(struct listener *l) } void -l_input_rc(struct listener *l) +l_input_rc(const struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); @@ -310,7 +310,7 @@ l_input_rc(struct listener *l) } void -l_global_position(struct listener *l) +l_global_position(const struct listener *l) { /* copy global position data into local buffer */ orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); @@ -340,7 +340,7 @@ l_global_position(struct listener *l) } void -l_local_position(struct listener *l) +l_local_position(const struct listener *l) { /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); @@ -357,7 +357,7 @@ l_local_position(struct listener *l) } void -l_global_position_setpoint(struct listener *l) +l_global_position_setpoint(const struct listener *l) { struct vehicle_global_position_setpoint_s global_sp; @@ -379,7 +379,7 @@ l_global_position_setpoint(struct listener *l) } void -l_local_position_setpoint(struct listener *l) +l_local_position_setpoint(const struct listener *l) { struct vehicle_local_position_setpoint_s local_sp; @@ -396,7 +396,7 @@ l_local_position_setpoint(struct listener *l) } void -l_attitude_setpoint(struct listener *l) +l_attitude_setpoint(const struct listener *l) { struct vehicle_attitude_setpoint_s att_sp; @@ -413,7 +413,7 @@ l_attitude_setpoint(struct listener *l) } void -l_vehicle_rates_setpoint(struct listener *l) +l_vehicle_rates_setpoint(const struct listener *l) { struct vehicle_rates_setpoint_s rates_sp; @@ -430,7 +430,7 @@ l_vehicle_rates_setpoint(struct listener *l) } void -l_actuator_outputs(struct listener *l) +l_actuator_outputs(const struct listener *l) { struct actuator_outputs_s act_outputs; @@ -529,13 +529,13 @@ l_actuator_outputs(struct listener *l) } void -l_actuator_armed(struct listener *l) +l_actuator_armed(const struct listener *l) { orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); } void -l_manual_control_setpoint(struct listener *l) +l_manual_control_setpoint(const struct listener *l) { struct manual_control_setpoint_s man_control; @@ -553,7 +553,7 @@ l_manual_control_setpoint(struct listener *l) } void -l_vehicle_attitude_controls(struct listener *l) +l_vehicle_attitude_controls(const struct listener *l) { struct actuator_controls_effective_s actuators; @@ -581,7 +581,7 @@ l_vehicle_attitude_controls(struct listener *l) } void -l_debug_key_value(struct listener *l) +l_debug_key_value(const struct listener *l) { struct debug_key_value_s debug; @@ -597,7 +597,7 @@ l_debug_key_value(struct listener *l) } void -l_optical_flow(struct listener *l) +l_optical_flow(const struct listener *l) { struct optical_flow_s flow; @@ -608,7 +608,7 @@ l_optical_flow(struct listener *l) } void -l_home(struct listener *l) +l_home(const struct listener *l) { struct home_position_s home; diff --git a/apps/px4/tests/test_bson.c b/apps/px4/tests/test_bson.c index a42c462ca..6130fe763 100644 --- a/apps/px4/tests/test_bson.c +++ b/apps/px4/tests/test_bson.c @@ -51,7 +51,7 @@ static const int32_t sample_small_int = 123; static const int64_t sample_big_int = (int64_t)INT_MAX + 123LL; static const double sample_double = 2.5f; static const char *sample_string = "this is a test"; -static const uint8_t sample_data[256]; +static const uint8_t sample_data[256] = {0}; //static const char *sample_filename = "/fs/microsd/bson.test"; static int diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index b507078a1..e80a41f15 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -192,6 +192,10 @@ controls_tick() { unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; ASSERT(mapped < MAX_CONTROL_CHANNELS); + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + scaled = -scaled; + r_rc_values[mapped] = SIGNED_TO_REG(scaled); assigned_channels |= (1 << mapped); } diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index ec69cdd64..f38593d2a 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -93,13 +93,11 @@ mixer_tick(void) /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - lowsyslog("AP RX timeout"); + isr_debug(1, "AP RX timeout"); } r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; - /* XXX this is questionable - vehicle may not make sense for direct control */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; @@ -179,7 +177,10 @@ mixer_tick(void) /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)); + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && + /* FMU is available or FMU is not available but override is an option */ + ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) + ); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 14d221b5b..8d8b7e941 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -76,7 +76,7 @@ #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) /* static configuration page */ -#define PX4IO_PAGE_CONFIG 0 +#define PX4IO_PAGE_CONFIG 0 #define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ @@ -88,7 +88,7 @@ #define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */ /* dynamic status page */ -#define PX4IO_PAGE_STATUS 1 +#define PX4IO_PAGE_STATUS 1 #define PX4IO_P_STATUS_FREEMEM 0 #define PX4IO_P_STATUS_CPULOAD 1 @@ -112,28 +112,33 @@ #define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ #define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ #define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ +#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ #define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ #define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */ /* array of post-mix actuator outputs, -10000..10000 */ -#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* array of PWM servo output values, microseconds */ -#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* array of raw RC input values, microseconds */ -#define PX4IO_PAGE_RAW_RC_INPUT 4 +#define PX4IO_PAGE_RAW_RC_INPUT 4 #define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */ #define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */ /* array of scaled RC input values, -10000..10000 */ -#define PX4IO_PAGE_RC_INPUT 5 +#define PX4IO_PAGE_RC_INPUT 5 #define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */ #define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */ /* array of raw ADC values */ -#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ +#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */ + +/* PWM servo information */ +#define PX4IO_PAGE_PWM_INFO 7 +#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ #define PX4IO_PAGE_SETUP 100 @@ -146,8 +151,8 @@ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ -#define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */ -#define PX4IO_P_SETUP_PWM_HIGHRATE 4 /* 'high' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ +#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ #define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ #define PX4IO_P_SETUP_IBATT_SCALE 7 /* battery current scaling factor (float) */ diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 7b4b07c2c..202e9d9d9 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -94,8 +94,8 @@ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ #define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES] #define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING] #define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] -#define r_setup_pwm_lowrate r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] -#define r_setup_pwm_highrate r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] +#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] +#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #define r_control_values (&r_page_controls[0]) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 645c1565d..6c09def9e 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -48,6 +48,7 @@ #include "protocol.h" static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value); +static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate); /** * PAGE 0 @@ -116,11 +117,12 @@ uint16_t r_page_rc_input[] = { }; /** - * PAGE 6 + * Scratch page; used for registers that are constructed as-read. * - * Raw ADC input. + * PAGE 6 Raw ADC input. + * PAGE 7 PWM rate maps. */ -uint16_t r_page_adc[ADC_CHANNEL_COUNT]; +uint16_t r_page_scratch[32]; /** * PAGE 100 @@ -132,8 +134,8 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_FEATURES] = 0, [PX4IO_P_SETUP_ARMING] = 0, [PX4IO_P_SETUP_PWM_RATES] = 0, - [PX4IO_P_SETUP_PWM_LOWRATE] = 50, - [PX4IO_P_SETUP_PWM_HIGHRATE] = 200, + [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, + [PX4IO_P_SETUP_PWM_ALTRATE] = 200, [PX4IO_P_SETUP_RELAYS] = 0, [PX4IO_P_SETUP_VBATT_SCALE] = 10000, [PX4IO_P_SETUP_IBATT_SCALE] = 0, @@ -321,26 +323,23 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_PWM_RATES: value &= PX4IO_P_SETUP_RATES_VALID; - r_setup_pwm_rates = value; - /* XXX re-configure timers */ + pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate); break; - case PX4IO_P_SETUP_PWM_LOWRATE: + case PX4IO_P_SETUP_PWM_DEFAULTRATE: if (value < 50) value = 50; if (value > 400) value = 400; - r_setup_pwm_lowrate = value; - /* XXX re-configure timers */ + pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; - case PX4IO_P_SETUP_PWM_HIGHRATE: + case PX4IO_P_SETUP_PWM_ALTRATE: if (value < 50) value = 50; if (value > 400) value = 400; - r_setup_pwm_highrate = value; - /* XXX re-configure timers */ + pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; case PX4IO_P_SETUP_RELAYS: @@ -529,10 +528,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val break; case PX4IO_PAGE_RAW_ADC_INPUT: - r_page_adc[0] = adc_measure(ADC_VBATT); - r_page_adc[1] = adc_measure(ADC_IN5); + memset(r_page_scratch, 0, sizeof(r_page_scratch)); + r_page_scratch[0] = adc_measure(ADC_VBATT); + r_page_scratch[1] = adc_measure(ADC_IN5); - SELECT_PAGE(r_page_adc); + SELECT_PAGE(r_page_scratch); + break; + + case PX4IO_PAGE_PWM_INFO: + memset(r_page_scratch, 0, sizeof(r_page_scratch)); + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); + + SELECT_PAGE(r_page_scratch); break; /* @@ -593,3 +601,44 @@ last_offset = offset; return 0; } + +/* + * Helper function to handle changes to the PWM rate control registers. + */ +static void +pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) +{ + for (unsigned pass = 0; pass < 2; pass++) { + for (unsigned group = 0; group < IO_SERVO_COUNT; group++) { + + /* get the channel mask for this rate group */ + uint32_t mask = up_pwm_servo_get_rate_group(group); + if (mask == 0) + continue; + + /* all channels in the group must be either default or alt-rate */ + uint32_t alt = map & mask; + + if (pass == 0) { + /* preflight */ + if ((alt != 0) && (alt != mask)) { + /* not a legal map, bail with an alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + return; + } + } else { + /* set it - errors here are unexpected */ + if (alt != 0) { + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } else { + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) + r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + } + } + } + r_setup_pwm_rates = map; + r_setup_pwm_defaultrate = defaultrate; + r_setup_pwm_altrate = altrate; +} diff --git a/apps/systemcmds/pwm/Makefile b/apps/systemcmds/pwm/Makefile new file mode 100644 index 000000000..5ab105ed1 --- /dev/null +++ b/apps/systemcmds/pwm/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Build the pwm tool. +# + +APPNAME = pwm +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +include $(APPDIR)/mk/app.mk diff --git a/apps/systemcmds/pwm/pwm.c b/apps/systemcmds/pwm/pwm.c new file mode 100644 index 000000000..de7a53199 --- /dev/null +++ b/apps/systemcmds/pwm/pwm.c @@ -0,0 +1,233 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pwm.c + * + * PWM servo output configuration and monitoring tool. + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> +#include <unistd.h> +#include <fcntl.h> +#include <sys/mount.h> +#include <sys/ioctl.h> +#include <sys/stat.h> + +#include <nuttx/i2c.h> +#include <nuttx/mtd.h> +#include <nuttx/fs/nxffs.h> +#include <nuttx/fs/ioctl.h> + +#include <arch/board/board.h> + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" +#include "drivers/drv_pwm_output.h" + +static void usage(const char *reason); +__EXPORT int pwm_main(int argc, char *argv[]); + + +static void +usage(const char *reason) +{ + if (reason != NULL) + warnx("%s", reason); + errx(1, + "usage:\n" + "pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [arm|disarm] [<channel_value> ...]\n" + " -v Print information about the PWM device\n" + " <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " <alt_rate> PWM update rate for channels in <alt_channel_mask>\n" + " <channel_group> Channel group that should update at the alternate rate (may be specified more than once)\n" + " arm | disarm Arm or disarm the ouptut\n" + " <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n" + "\n" + "When -c is specified, any channel groups not listed with -c will update at the default rate.\n" + ); + +} + +int +pwm_main(int argc, char *argv[]) +{ + const char *dev = PWM_OUTPUT_DEVICE_PATH; + unsigned alt_rate = 0; + uint32_t alt_channel_groups = 0; + bool alt_channels_set = false; + bool print_info = false; + int ch; + int ret; + char *ep; + unsigned group; + + if (argc < 2) + usage(NULL); + + while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) { + switch (ch) { + case 'c': + group = strtoul(optarg, &ep, 0); + if ((*ep != '\0') || (group >= 32)) + usage("bad channel_group value"); + alt_channel_groups |= (1 << group); + alt_channels_set = true; + break; + + case 'd': + dev = optarg; + break; + + case 'u': + alt_rate = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad alt_rate value"); + break; + + case 'v': + print_info = true; + break; + + default: + usage(NULL); + } + } + argc -= optind; + argv += optind; + + /* open for ioctl only */ + int fd = open(dev, 0); + if (fd < 0) + err(1, "can't open %s", dev); + + /* change alternate PWM rate */ + if (alt_rate > 0) { + ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); + if (ret != OK) + err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); + } + + /* assign alternate rate to channel groups */ + if (alt_channels_set) { + uint32_t mask = 0; + + for (unsigned group = 0; group < 32; group++) { + if ((1 << group) & alt_channel_groups) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask); + if (ret != OK) + err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); + + mask |= group_mask; + } + } + + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); + } + + /* iterate remaining arguments */ + unsigned channel = 0; + while (argc--) { + const char *arg = argv[0]; + argv++; + if (!strcmp(arg, "arm")) { + ret = ioctl(fd, PWM_SERVO_ARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_ARM"); + continue; + } + if (!strcmp(arg, "disarm")) { + ret = ioctl(fd, PWM_SERVO_DISARM, 0); + if (ret != OK) + err(1, "PWM_SERVO_DISARM"); + continue; + } + unsigned pwm_value = strtol(arg, &ep, 0); + if (*ep == '\0') { + ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value); + if (ret != OK) + err(1, "PWM_SERVO_SET(%d)", channel); + channel++; + continue; + } + usage("unrecognised option"); + } + + /* print verbose info */ + if (print_info) { + /* get the number of servo channels */ + unsigned count; + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count); + if (ret != OK) + err(1, "PWM_SERVO_GET_COUNT"); + + /* print current servo values */ + for (unsigned i = 0; i < count; i++) { + servo_position_t spos; + + ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); + if (ret == OK) { + printf("channel %u: %uus\n", i, spos); + } else { + printf("%u: ERROR\n", i); + } + } + + /* print rate groups */ + for (unsigned i = 0; i < count; i++) { + uint32_t group_mask; + + ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask); + if (ret != OK) + break; + if (group_mask != 0) { + printf("channel group %u: channels", i); + for (unsigned j = 0; j < 32; j++) + if (group_mask & (1 << j)) + printf(" %u", j); + printf("\n"); + } + } + fflush(stdout); + } + exit(0); +}
\ No newline at end of file diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index ebb72ca3e..8073570d1 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -87,7 +87,7 @@ struct param_wbuf_s { UT_array *param_values; /** array info for the modified parameters array */ -UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; +const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; /** parameter update topic */ ORB_DEFINE(parameter_update, struct parameter_update_s); diff --git a/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c b/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c index bcce3ce60..2c9ae4cac 100644 --- a/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c +++ b/nuttx/arch/arm/src/stm32/stm32_otgfsdev.c @@ -397,7 +397,6 @@ struct stm32_ep_s struct stm32_req_s *tail; uint8_t epphy; /* Physical EP address */ uint8_t eptype:2; /* Endpoint type */ - uint8_t configured:1; /* 1: Endpoint has been configured */ uint8_t active:1; /* 1: A request is being processed */ uint8_t stalled:1; /* 1: Endpoint is stalled */ uint8_t isin:1; /* 1: IN Endpoint */ @@ -679,6 +678,99 @@ static const struct usbdev_ops_s g_devops = .pullup = stm32_pullup, }; +/* Device error strings that may be enabled for more desciptive USB trace + * output. + */ + +#ifdef CONFIG_USBDEV_TRACE_STRINGS +const struct trace_msg_t g_usb_trace_strings_deverror[] = +{ + TRACE_STR(STM32_TRACEERR_ALLOCFAIL ), + TRACE_STR(STM32_TRACEERR_BADCLEARFEATURE ), + TRACE_STR(STM32_TRACEERR_BADDEVGETSTATUS ), + TRACE_STR(STM32_TRACEERR_BADEPNO ), + TRACE_STR(STM32_TRACEERR_BADEPGETSTATUS ), + TRACE_STR(STM32_TRACEERR_BADGETCONFIG ), + TRACE_STR(STM32_TRACEERR_BADGETSETDESC ), + TRACE_STR(STM32_TRACEERR_BADGETSTATUS ), + TRACE_STR(STM32_TRACEERR_BADSETADDRESS ), + TRACE_STR(STM32_TRACEERR_BADSETCONFIG ), + TRACE_STR(STM32_TRACEERR_BADSETFEATURE ), + TRACE_STR(STM32_TRACEERR_BADTESTMODE ), + TRACE_STR(STM32_TRACEERR_BINDFAILED ), + TRACE_STR(STM32_TRACEERR_DISPATCHSTALL ), + TRACE_STR(STM32_TRACEERR_DRIVER ), + TRACE_STR(STM32_TRACEERR_DRIVERREGISTERED), + TRACE_STR(STM32_TRACEERR_EP0NOSETUP ), + TRACE_STR(STM32_TRACEERR_EP0SETUPSTALLED ), + TRACE_STR(STM32_TRACEERR_EPINNULLPACKET ), + TRACE_STR(STM32_TRACEERR_EPOUTNULLPACKET ), + TRACE_STR(STM32_TRACEERR_INVALIDCTRLREQ ), + TRACE_STR(STM32_TRACEERR_INVALIDPARMS ), + TRACE_STR(STM32_TRACEERR_IRQREGISTRATION ), + TRACE_STR(STM32_TRACEERR_NOEP ), + TRACE_STR(STM32_TRACEERR_NOTCONFIGURED ), + TRACE_STR(STM32_TRACEERR_EPOUTQEMPTY ), + TRACE_STR(STM32_TRACEERR_EPINREQEMPTY ), + TRACE_STR(STM32_TRACEERR_NOOUTSETUP ), + TRACE_STR(STM32_TRACEERR_POLLTIMEOUT ), + TRACE_STR_END +}; +#endif + +/* Interrupt event strings that may be enabled for more desciptive USB trace + * output. + */ + +#ifdef CONFIG_USBDEV_TRACE_STRINGS +const struct trace_msg_t g_usb_trace_strings_intdecode[] = +{ + TRACE_STR(STM32_TRACEINTID_USB ), + TRACE_STR(STM32_TRACEINTID_INTPENDING ), + TRACE_STR(STM32_TRACEINTID_EPOUT ), + TRACE_STR(STM32_TRACEINTID_EPIN ), + TRACE_STR(STM32_TRACEINTID_MISMATCH ), + TRACE_STR(STM32_TRACEINTID_WAKEUP ), + TRACE_STR(STM32_TRACEINTID_SUSPEND ), + TRACE_STR(STM32_TRACEINTID_SOF ), + TRACE_STR(STM32_TRACEINTID_RXFIFO ), + TRACE_STR(STM32_TRACEINTID_DEVRESET ), + TRACE_STR(STM32_TRACEINTID_ENUMDNE ), + TRACE_STR(STM32_TRACEINTID_IISOIXFR ), + TRACE_STR(STM32_TRACEINTID_IISOOXFR ), + TRACE_STR(STM32_TRACEINTID_SRQ ), + TRACE_STR(STM32_TRACEINTID_OTG ), + TRACE_STR(STM32_TRACEINTID_EPOUT_XFRC ), + TRACE_STR(STM32_TRACEINTID_EPOUT_EPDISD), + TRACE_STR(STM32_TRACEINTID_EPOUT_SETUP ), + TRACE_STR(STM32_TRACEINTID_DISPATCH ), + TRACE_STR(STM32_TRACEINTID_GETSTATUS ), + TRACE_STR(STM32_TRACEINTID_EPGETSTATUS ), + TRACE_STR(STM32_TRACEINTID_DEVGETSTATUS), + TRACE_STR(STM32_TRACEINTID_IFGETSTATUS ), + TRACE_STR(STM32_TRACEINTID_CLEARFEATURE), + TRACE_STR(STM32_TRACEINTID_SETFEATURE ), + TRACE_STR(STM32_TRACEINTID_SETADDRESS ), + TRACE_STR(STM32_TRACEINTID_GETSETDESC ), + TRACE_STR(STM32_TRACEINTID_GETCONFIG ), + TRACE_STR(STM32_TRACEINTID_SETCONFIG ), + TRACE_STR(STM32_TRACEINTID_GETSETIF ), + TRACE_STR(STM32_TRACEINTID_SYNCHFRAME ), + TRACE_STR(STM32_TRACEINTID_EPIN_XFRC ), + TRACE_STR(STM32_TRACEINTID_EPIN_TOC ), + TRACE_STR(STM32_TRACEINTID_EPIN_ITTXFE ), + TRACE_STR(STM32_TRACEINTID_EPIN_EPDISD ), + TRACE_STR(STM32_TRACEINTID_EPIN_TXFE ), + TRACE_STR(STM32_TRACEINTID_EPIN_EMPWAIT), + TRACE_STR(STM32_TRACEINTID_OUTNAK ), + TRACE_STR(STM32_TRACEINTID_OUTRECVD ), + TRACE_STR(STM32_TRACEINTID_OUTDONE ), + TRACE_STR(STM32_TRACEINTID_SETUPDONE ), + TRACE_STR(STM32_TRACEINTID_SETUPRECVD ), + TRACE_STR_END +}; +#endif + /******************************************************************************* * Public Data *******************************************************************************/ @@ -1142,7 +1234,7 @@ static void stm32_epin_request(FAR struct stm32_usbdev_s *priv, /* Add one more packet to the TxFIFO. We will wait for the transfer * complete event before we add the next packet (or part of a packet * to the TxFIFO). - * + * * The documentation says that we can can multiple packets to the TxFIFO, * but it seems that we need to get the transfer complete event before * we can add the next (or maybe I have got something wrong?) @@ -1796,13 +1888,6 @@ static struct stm32_ep_s *stm32_ep_findbyaddr(struct stm32_usbdev_s *priv, privep = &priv->epout[epphy]; } - /* Verify that the endpoint has been configured */ - - if (!privep->configured) - { - return NULL; - } - /* Return endpoint reference */ DEBUGASSERT(privep->epphy == epphy); @@ -2459,16 +2544,16 @@ static inline void stm32_epout(FAR struct stm32_usbdev_s *priv, uint8_t epno) /* Continue processing data from the EP0 OUT request queue */ stm32_epout_complete(priv, privep); - } - /* If we are not actively processing an OUT request, then we - * need to setup to receive the next control request. - */ + /* If we are not actively processing an OUT request, then we + * need to setup to receive the next control request. + */ - if (!privep->active) - { - stm32_ep0out_ctrlsetup(priv); - priv->ep0state = EP0STATE_IDLE; + if (!privep->active) + { + stm32_ep0out_ctrlsetup(priv); + priv->ep0state = EP0STATE_IDLE; + } } } @@ -2626,16 +2711,16 @@ static inline void stm32_epin(FAR struct stm32_usbdev_s *priv, uint8_t epno) /* Continue processing data from the EP0 OUT request queue */ stm32_epin_request(priv, privep); - } - /* If we are not actively processing an OUT request, then we - * need to setup to receive the next control request. - */ + /* If we are not actively processing an OUT request, then we + * need to setup to receive the next control request. + */ - if (!privep->active) - { - stm32_ep0out_ctrlsetup(priv); - priv->ep0state = EP0STATE_IDLE; + if (!privep->active) + { + stm32_ep0out_ctrlsetup(priv); + priv->ep0state = EP0STATE_IDLE; + } } /* Test mode is another special case */ @@ -2754,7 +2839,7 @@ static inline void stm32_epin_interrupt(FAR struct stm32_usbdev_s *priv) * interrupt here; it will be re-enabled if there is still * insufficient space in the TxFIFO. */ - + empty &= ~OTGFS_DIEPEMPMSK(epno); stm32_putreg(empty, STM32_OTGFS_DIEPEMPMSK); stm32_putreg(OTGFS_DIEPINT_XFRC, STM32_OTGFS_DIEPINT(epno)); @@ -3063,6 +3148,12 @@ static inline void stm32_rxinterrupt(FAR struct stm32_usbdev_s *priv) datlen = GETUINT16(priv->ctrlreq.len); if (USB_REQ_ISOUT(priv->ctrlreq.type) && datlen > 0) { + /* Clear NAKSTS so that we can receive the data */ + + regval = stm32_getreg(STM32_OTGFS_DOEPCTL0); + regval |= OTGFS_DOEPCTL0_CNAK; + stm32_putreg(regval, STM32_OTGFS_DOEPCTL0); + /* Wait for the data phase. */ priv->ep0state = EP0STATE_SETUP_OUT; @@ -3654,7 +3745,7 @@ static int stm32_epout_configure(FAR struct stm32_ep_s *privep, uint8_t eptype, { regval |= OTGFS_DOEPCTL_CNAK; } - + regval &= ~(OTGFS_DOEPCTL_MPSIZ_MASK | OTGFS_DOEPCTL_EPTYP_MASK); regval |= mpsiz; regval |= (eptype << OTGFS_DOEPCTL_EPTYP_SHIFT); @@ -3750,7 +3841,7 @@ static int stm32_epin_configure(FAR struct stm32_ep_s *privep, uint8_t eptype, { regval |= OTGFS_DIEPCTL_CNAK; } - + regval &= ~(OTGFS_DIEPCTL_MPSIZ_MASK | OTGFS_DIEPCTL_EPTYP_MASK | OTGFS_DIEPCTL_TXFNUM_MASK); regval |= mpsiz; regval |= (eptype << OTGFS_DIEPCTL_EPTYP_SHIFT); @@ -4345,19 +4436,6 @@ static int stm32_epin_setstall(FAR struct stm32_ep_s *privep) regaddr = STM32_OTGFS_DIEPCTL(privep->epphy); regval = stm32_getreg(regaddr); - /* Is the endpoint enabled? */ - - if ((regval & OTGFS_DIEPCTL_EPENA) != 0) - { - /* Yes.. the endpoint is enabled, disable it */ - - regval = OTGFS_DIEPCTL_EPDIS; - } - else - { - regval = 0; - } - /* Then stall the endpoint */ regval |= OTGFS_DIEPCTL_STALL; diff --git a/nuttx/arch/arm/src/stm32/stm32_usbdev.c b/nuttx/arch/arm/src/stm32/stm32_usbdev.c index d13ac8f96..6036eb3d5 100644 --- a/nuttx/arch/arm/src/stm32/stm32_usbdev.c +++ b/nuttx/arch/arm/src/stm32/stm32_usbdev.c @@ -1,7 +1,7 @@ /**************************************************************************** * arch/arm/src/stm32/stm32_usbdev.c * - * Copyright (C) 2009-2012 Gregory Nutt. All rights reserved. + * Copyright (C) 2009-2013 Gregory Nutt. All rights reserved. * Author: Gregory Nutt <gnutt@nuttx.orgr> * * References: @@ -59,7 +59,9 @@ #include <arch/irq.h> #include "up_arch.h" -#include "stm32_internal.h" +#include "stm32.h" +#include "stm32_syscfg.h" +#include "stm32_gpio.h" #include "stm32_usbdev.h" #if defined(CONFIG_USBDEV) && defined(CONFIG_STM32_USB) @@ -78,6 +80,20 @@ # define CONFIG_USB_PRI NVIC_SYSH_PRIORITY_DEFAULT #endif +/* USB Interrupts. Should be re-mapped if CAN is used. */ + +#ifdef CONFIG_STM32_STM32F30XX +# ifdef CONFIG_STM32_USB_ITRMP +# define STM32_IRQ_USBHP STM32_IRQ_USBHP_2 +# define STM32_IRQ_USBLP STM32_IRQ_USBLP_2 +# define STM32_IRQ_USBWKUP STM32_IRQ_USBWKUP_2 +# else +# define STM32_IRQ_USBHP STM32_IRQ_USBHP_1 +# define STM32_IRQ_USBLP STM32_IRQ_USBLP_1 +# define STM32_IRQ_USBWKUP STM32_IRQ_USBWKUP_1 +# endif +#endif + /* Extremely detailed register debug that you would normally never want * enabled. */ @@ -250,12 +266,12 @@ /* The various states of a control pipe */ -enum stm32_devstate_e +enum stm32_ep0state_e { - DEVSTATE_IDLE = 0, /* No request in progress */ - DEVSTATE_RDREQUEST, /* Read request in progress */ - DEVSTATE_WRREQUEST, /* Write request in progress */ - DEVSTATE_STALLED /* We are stalled */ + EP0STATE_IDLE = 0, /* No request in progress */ + EP0STATE_RDREQUEST, /* Read request in progress */ + EP0STATE_WRREQUEST, /* Write request in progress */ + EP0STATE_STALLED /* We are stalled */ }; /* Resume states */ @@ -320,7 +336,7 @@ struct stm32_usbdev_s /* STM32-specific fields */ struct usb_ctrlreq_s ctrl; /* Last EP0 request */ - uint8_t devstate; /* Driver state (see enum stm32_devstate_e) */ + uint8_t ep0state; /* State of EP0 (see enum stm32_ep0state_e) */ uint8_t rsmstate; /* Resume state (see enum stm32_rsmstate_e) */ uint8_t nesofs; /* ESOF counter (for resume support) */ uint8_t rxpending:1; /* 1: OUT data in PMA, but no read requests */ @@ -1014,7 +1030,7 @@ static void stm32_copytopma(const uint8_t *buffer, uint16_t pma, uint16_t nbytes /* Copy loop. Source=user buffer, Dest=packet memory */ - dest = (uint16_t*)(STM32_USBCANRAM_BASE + ((uint32_t)pma << 1)); + dest = (uint16_t*)(STM32_USBRAM_BASE + ((uint32_t)pma << 1)); for (i = nwords; i != 0; i--) { /* Read two bytes and pack into on 16-bit word */ @@ -1044,7 +1060,7 @@ stm32_copyfrompma(uint8_t *buffer, uint16_t pma, uint16_t nbytes) /* Copy loop. Source=packet memory, Dest=user buffer */ - src = (uint32_t*)(STM32_USBCANRAM_BASE + ((uint32_t)pma << 1)); + src = (uint32_t*)(STM32_USBRAM_BASE + ((uint32_t)pma << 1)); for (i = nwords; i != 0; i--) { /* Copy 16-bits from packet memory to user buffer. */ @@ -1142,7 +1158,7 @@ static void stm32_reqcomplete(struct stm32_ep_s *privep, int16_t result) bool stalled = privep->stalled; if (USB_EPNO(privep->ep.eplog) == EP0) { - privep->stalled = (privep->dev->devstate == DEVSTATE_STALLED); + privep->stalled = (privep->dev->ep0state == EP0STATE_STALLED); } /* Save the result in the request structure */ @@ -1222,8 +1238,7 @@ static int stm32_wrrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive */ usbtrace(TRACE_INTDECODE(STM32_TRACEINTID_EPINQEMPTY), 0); - priv->devstate = DEVSTATE_IDLE; - return OK; + return -ENOENT; } epno = USB_EPNO(privep->ep.eplog); @@ -1267,7 +1282,6 @@ static int stm32_wrrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive buf = privreq->req.buf + privreq->req.xfrd; stm32_epwrite(priv, privep, buf, nbytes); - priv->devstate = DEVSTATE_WRREQUEST; /* Update for the next data IN interrupt */ @@ -1275,15 +1289,16 @@ static int stm32_wrrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive bytesleft = privreq->req.len - privreq->req.xfrd; /* If all of the bytes were sent (including any final null packet) - * then we are finished with the transfer + * then we are finished with the request buffer). */ if (bytesleft == 0 && !privep->txnullpkt) { + /* Return the write request to the class driver */ + usbtrace(TRACE_COMPLETE(USB_EPNO(privep->ep.eplog)), privreq->req.xfrd); privep->txnullpkt = 0; stm32_reqcomplete(privep, OK); - priv->devstate = DEVSTATE_IDLE; } return OK; @@ -1314,7 +1329,7 @@ static int stm32_rdrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive */ usbtrace(TRACE_INTDECODE(STM32_TRACEINTID_EPOUTQEMPTY), epno); - return OK; + return -ENOENT; } ullvdbg("EP%d: len=%d xfrd=%d\n", epno, privreq->req.len, privreq->req.xfrd); @@ -1343,22 +1358,18 @@ static int stm32_rdrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive /* Receive the next packet */ stm32_copyfrompma(dest, src, readlen); - priv->devstate = DEVSTATE_RDREQUEST; /* If the receive buffer is full or this is a partial packet, - * then we are finished with the transfer + * then we are finished with the request buffer). */ privreq->req.xfrd += readlen; if (pmalen < privep->ep.maxpacket || privreq->req.xfrd >= privreq->req.len) { - /* Complete the transfer and mark the state IDLE. The endpoint - * RX will be marked valid when the data phase completes. - */ + /* Return the read request to the class driver. */ usbtrace(TRACE_COMPLETE(epno), privreq->req.xfrd); stm32_reqcomplete(privep, OK); - priv->devstate = DEVSTATE_IDLE; } return OK; @@ -1400,7 +1411,7 @@ static void stm32_dispatchrequest(struct stm32_usbdev_s *priv) /* Stall on failure */ usbtrace(TRACE_DEVERROR(STM32_TRACEERR_DISPATCHSTALL), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } } @@ -1436,7 +1447,7 @@ static void stm32_epdone(struct stm32_usbdev_s *priv, uint8_t epno) { /* Read host data into the current read request */ - stm32_rdrequest(priv, privep); + (void)stm32_rdrequest(priv, privep); /* "After the received data is processed, the application software * should set the STAT_RX bits to '11' (Valid) in the USB_EPnR, @@ -1567,7 +1578,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) ullvdbg("SETUP: type=%02x req=%02x value=%04x index=%04x len=%04x\n", priv->ctrl.type, priv->ctrl.req, value.w, index.w, len.w); - priv->devstate = DEVSTATE_IDLE; + priv->ep0state = EP0STATE_IDLE; /* Dispatch any non-standard requests */ @@ -1600,7 +1611,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) index.b[MSB] != 0 || value.w != 0) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADEPGETSTATUS), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } else { @@ -1613,7 +1624,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) if (epno >= STM32_NENDPOINTS) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADEPGETSTATUS), epno); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } else { @@ -1663,7 +1674,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADDEVGETSTATUS), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } break; @@ -1679,7 +1690,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) default: { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADGETSTATUS), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } break; } @@ -1720,7 +1731,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADCLEARFEATURE), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } } @@ -1764,7 +1775,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADSETFEATURE), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } } @@ -1783,7 +1794,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) index.w != 0 || len.w != 0 || value.w > 127) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADSETADDRESS), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } /* Note that setting of the device address will be deferred. A zero-length @@ -1818,7 +1829,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADGETSETDESC), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } break; @@ -1843,7 +1854,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADGETCONFIG), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } break; @@ -1868,7 +1879,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) else { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADSETCONFIG), 0); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } break; @@ -1910,7 +1921,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) default: { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_INVALIDCTRLREQ), priv->ctrl.req); - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } break; } @@ -1922,9 +1933,9 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) * must be sent (may be a zero length packet). * 2. The request was successfully handled by the class implementation. In * case, the EP0 IN response has already been queued and the local variable - * 'handled' will be set to true and devstate != DEVSTATE_STALLED; + * 'handled' will be set to true and ep0state != EP0STATE_STALLED; * 3. An error was detected in either the above logic or by the class implementation - * logic. In either case, priv->state will be set DEVSTATE_STALLED + * logic. In either case, priv->state will be set EP0STATE_STALLED * to indicate this case. * * NOTE: Non-standard requests are a special case. They are handled by the @@ -1932,7 +1943,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) * logic altogether. */ - if (priv->devstate != DEVSTATE_STALLED && !handled) + if (priv->ep0state != EP0STATE_STALLED && !handled) { /* We will response. First, restrict the data length to the length * requested in the setup packet @@ -1946,7 +1957,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) /* Send the response (might be a zero-length packet) */ stm32_epwrite(priv, ep0, response.b, nbytes); - priv->devstate = DEVSTATE_IDLE; + priv->ep0state = EP0STATE_IDLE; } } @@ -1956,6 +1967,8 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv) static void stm32_ep0in(struct stm32_usbdev_s *priv) { + int ret; + /* There is no longer anything in the EP0 TX packet memory */ priv->eplist[EP0].txbusy = false; @@ -1964,14 +1977,15 @@ static void stm32_ep0in(struct stm32_usbdev_s *priv) * from the class driver? */ - if (priv->devstate == DEVSTATE_WRREQUEST) + if (priv->ep0state == EP0STATE_WRREQUEST) { - stm32_wrrequest(priv, &priv->eplist[EP0]); + ret = stm32_wrrequest(priv, &priv->eplist[EP0]); + priv->ep0state = ((ret == OK) ? EP0STATE_WRREQUEST : EP0STATE_IDLE); } /* No.. Are we processing the completion of a status response? */ - else if (priv->devstate == DEVSTATE_IDLE) + else if (priv->ep0state == EP0STATE_IDLE) { /* Look at the saved SETUP command. Was it a SET ADDRESS request? * If so, then now is the time to set the address. @@ -1987,7 +2001,7 @@ static void stm32_ep0in(struct stm32_usbdev_s *priv) } else { - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; } } @@ -1997,12 +2011,15 @@ static void stm32_ep0in(struct stm32_usbdev_s *priv) static void stm32_ep0out(struct stm32_usbdev_s *priv) { + int ret; + struct stm32_ep_s *privep = &priv->eplist[EP0]; - switch (priv->devstate) + switch (priv->ep0state) { - case DEVSTATE_RDREQUEST: /* Write request in progress */ - case DEVSTATE_IDLE: /* No transfer in progress */ - stm32_rdrequest(priv, privep); + case EP0STATE_RDREQUEST: /* Write request in progress */ + case EP0STATE_IDLE: /* No transfer in progress */ + ret = stm32_rdrequest(priv, privep); + priv->ep0state = ((ret == OK) ? EP0STATE_RDREQUEST : EP0STATE_IDLE); break; default: @@ -2010,7 +2027,7 @@ static void stm32_ep0out(struct stm32_usbdev_s *priv) * completed, STALL the endpoint in either case */ - priv->devstate = DEVSTATE_STALLED; + priv->ep0state = EP0STATE_STALLED; break; } } @@ -2103,7 +2120,7 @@ static inline void stm32_ep0done(struct stm32_usbdev_s *priv, uint16_t istr) /* Now figure out the new RX/TX status. Here are all possible * consequences of the above EP0 operations: * - * rxstatus txstatus devstate MEANING + * rxstatus txstatus ep0state MEANING * -------- -------- --------- --------------------------------- * NAK NAK IDLE Nothing happened * NAK VALID IDLE EP0 response sent from USBDEV driver @@ -2113,9 +2130,9 @@ static inline void stm32_ep0done(struct stm32_usbdev_s *priv, uint16_t istr) * First handle the STALL condition: */ - if (priv->devstate == DEVSTATE_STALLED) + if (priv->ep0state == EP0STATE_STALLED) { - usbtrace(TRACE_DEVERROR(STM32_TRACEERR_EP0SETUPSTALLED), priv->devstate); + usbtrace(TRACE_DEVERROR(STM32_TRACEERR_EP0SETUPSTALLED), priv->ep0state); priv->rxstatus = USB_EPR_STATRX_STALL; priv->txstatus = USB_EPR_STATTX_STALL; } @@ -2843,7 +2860,7 @@ static int stm32_epsubmit(struct usbdev_ep_s *ep, struct usbdev_req_s *req) if (!privep->txbusy) { priv->txstatus = USB_EPR_STATTX_NAK; - ret = stm32_wrrequest(priv, privep); + ret = stm32_wrrequest(priv, privep); /* Set the new TX status */ @@ -2955,7 +2972,12 @@ static int stm32_epstall(struct usbdev_ep_s *ep, bool resume) if (status == 0) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_EPDISABLED), 0); - priv->devstate = DEVSTATE_STALLED; + + if (epno == 0) + { + priv->ep0state = EP0STATE_STALLED; + } + return -ENODEV; } @@ -3256,7 +3278,7 @@ static void stm32_reset(struct stm32_usbdev_s *priv) /* Reset the device state structure */ - priv->devstate = DEVSTATE_IDLE; + priv->ep0state = EP0STATE_IDLE; priv->rsmstate = RSMSTATE_IDLE; priv->rxpending = false; @@ -3466,27 +3488,48 @@ void up_usbinitialize(void) usbtrace(TRACE_DEVINIT, 0); stm32_checksetup(); + /* Configure USB GPIO alternate function pins */ + +#ifdef CONFIG_STM32_STM32F30XX + (void)stm32_configgpio(GPIO_USB_DM); + (void)stm32_configgpio(GPIO_USB_DP); +#endif + /* Power up the USB controller, but leave it in the reset state */ stm32_hwsetup(priv); + /* Remap the USB interrupt as needed (Only supported by the STM32 F3 family) */ + +#ifdef CONFIG_STM32_STM32F30XX +# ifdef CONFIG_STM32_USB_ITRMP + /* Clear the ITRMP bit to use the legacy, shared USB/CAN interrupts */ + + modifyreg32(STM32_RCC_APB1ENR, SYSCFG_CFGR1_USB_ITRMP, 0); +# else + /* Set the ITRMP bit to use the STM32 F3's dedicated USB interrupts */ + + modifyreg32(STM32_RCC_APB1ENR, 0, SYSCFG_CFGR1_USB_ITRMP); +# endif +#endif + /* Attach USB controller interrupt handlers. The hardware will not be * initialized and interrupts will not be enabled until the class device * driver is bound. Getting the IRQs here only makes sure that we have * them when we need them later. */ - if (irq_attach(STM32_IRQ_USBHPCANTX, stm32_hpinterrupt) != 0) + if (irq_attach(STM32_IRQ_USBHP, stm32_hpinterrupt) != 0) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_IRQREGISTRATION), - (uint16_t)STM32_IRQ_USBHPCANTX); + (uint16_t)STM32_IRQ_USBHP); goto errout; } - if (irq_attach(STM32_IRQ_USBLPCANRX0, stm32_lpinterrupt) != 0) + if (irq_attach(STM32_IRQ_USBLP, stm32_lpinterrupt) != 0) { usbtrace(TRACE_DEVERROR(STM32_TRACEERR_IRQREGISTRATION), - (uint16_t)STM32_IRQ_USBLPCANRX0); + (uint16_t)STM32_IRQ_USBLP); goto errout; } return; @@ -3522,10 +3565,10 @@ void up_usbuninitialize(void) /* Disable and detach the USB IRQs */ - up_disable_irq(STM32_IRQ_USBHPCANTX); - up_disable_irq(STM32_IRQ_USBLPCANRX0); - irq_detach(STM32_IRQ_USBHPCANTX); - irq_detach(STM32_IRQ_USBLPCANRX0); + up_disable_irq(STM32_IRQ_USBHP); + up_disable_irq(STM32_IRQ_USBLP); + irq_detach(STM32_IRQ_USBHP); + irq_detach(STM32_IRQ_USBLP); if (priv->driver) { @@ -3595,13 +3638,13 @@ int usbdev_register(struct usbdevclass_driver_s *driver) /* Enable USB controller interrupts at the NVIC */ - up_enable_irq(STM32_IRQ_USBHPCANTX); - up_enable_irq(STM32_IRQ_USBLPCANRX0); + up_enable_irq(STM32_IRQ_USBHP); + up_enable_irq(STM32_IRQ_USBLP); /* Set the interrrupt priority */ - up_prioritize_irq(STM32_IRQ_USBHPCANTX, CONFIG_USB_PRI); - up_prioritize_irq(STM32_IRQ_USBLPCANRX0, CONFIG_USB_PRI); + up_prioritize_irq(STM32_IRQ_USBHP, CONFIG_USB_PRI); + up_prioritize_irq(STM32_IRQ_USBLP, CONFIG_USB_PRI); /* Enable pull-up to connect the device. The host should enumerate us * some time after this @@ -3657,8 +3700,8 @@ int usbdev_unregister(struct usbdevclass_driver_s *driver) /* Disable USB controller interrupts (but keep them attached) */ - up_disable_irq(STM32_IRQ_USBHPCANTX); - up_disable_irq(STM32_IRQ_USBLPCANRX0); + up_disable_irq(STM32_IRQ_USBHP); + up_disable_irq(STM32_IRQ_USBLP); /* Put the hardware in an inactive state. Then bring the hardware back up * in the reset state (this is probably not necessary, the stm32_reset() diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index f1f70e228..80cf6f312 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -67,6 +67,7 @@ CONFIGURED_APPS += systemcmds/boardinfo CONFIGURED_APPS += systemcmds/mixer CONFIGURED_APPS += systemcmds/eeprom CONFIGURED_APPS += systemcmds/param +CONFIGURED_APPS += systemcmds/pwm CONFIGURED_APPS += systemcmds/bl_update CONFIGURED_APPS += systemcmds/preflight_check CONFIGURED_APPS += systemcmds/delay_test diff --git a/nuttx/libc/stdio/lib_sscanf.c b/nuttx/libc/stdio/lib_sscanf.c index 77a6cf212..0092fbec2 100644 --- a/nuttx/libc/stdio/lib_sscanf.c +++ b/nuttx/libc/stdio/lib_sscanf.c @@ -197,7 +197,7 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) noassign = false; lflag = false; - while (*fmt && *buf) + while (*fmt) { /* Skip over white space */ @@ -242,6 +242,33 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) fmt--; } } + + /* Process %n: Character count */ + + if (*fmt == 'n') + { + lvdbg("vsscanf: Performing character count\n"); + + if (!noassign) + { + size_t nchars = (size_t)(buf - bufstart); + + if (lflag) + { + long *plong = va_arg(ap, long*); + *plong = (long)nchars; + } + else + { + int *pint = va_arg(ap, int*); + *pint = (int)nchars; + } + } + } else { + + /* Check for valid data in input string */ + if (!(*buf)) + break; /* Process %s: String conversion */ @@ -445,28 +472,7 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap) #endif } - /* Process %n: Character count */ - - else if (*fmt == 'n') - { - lvdbg("vsscanf: Performing character count\n"); - - if (!noassign) - { - size_t nchars = (size_t)(buf - bufstart); - - if (lflag) - { - long *plong = va_arg(ap, long*); - *plong = (long)nchars; - } - else - { - int *pint = va_arg(ap, int*); - *pint = (int)nchars; - } - } - } + } /* Note %n does not count as a conversion */ |