diff options
28 files changed, 1078 insertions, 494 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/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/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 8e13f7c62..476adb7f0 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,30 @@ 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; + _current_update_rate = 500; } /* reject slower than 50 Hz updates */ if (update_rate_in_ms > 20) { update_rate_in_ms = 20; - _update_rate = 50; + _current_update_rate = 50; } + 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; } /* sleep waiting for data, stopping to check for PPM @@ -527,7 +593,6 @@ int PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) { int ret = OK; - int channel; lock(); @@ -541,7 +606,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 +624,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 +640,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 +885,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 +937,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 +1032,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 +1067,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 791964087..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: @@ -122,7 +135,7 @@ private: uint16_t _alarms; /* subscribed topics */ - int _t_actuators; ///< actuator output topic + int _t_actuators; ///< actuator controls topic int _t_armed; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic @@ -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/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 58c709aec..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; @@ -513,14 +513,14 @@ l_actuator_outputs(struct listener *l) } else { mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), - (act_outputs.output[0] - 1500.0f) / 600.0f, - (act_outputs.output[1] - 1500.0f) / 600.0f, - (act_outputs.output[2] - 1500.0f) / 600.0f, - (act_outputs.output[3] - 900.0f) / 1200.0f, - (act_outputs.output[4] - 1500.0f) / 600.0f, - (act_outputs.output[5] - 1500.0f) / 600.0f, - (act_outputs.output[6] - 1500.0f) / 600.0f, - (act_outputs.output[7] - 1500.0f) / 600.0f, + (act_outputs.output[0] - 1500.0f) / 500.0f, + (act_outputs.output[1] - 1500.0f) / 500.0f, + (act_outputs.output[2] - 1500.0f) / 500.0f, + (act_outputs.output[3] - 1000.0f) / 1000.0f, + (act_outputs.output[4] - 1500.0f) / 500.0f, + (act_outputs.output[5] - 1500.0f) / 500.0f, + (act_outputs.output[6] - 1500.0f) / 500.0f, + (act_outputs.output[7] - 1500.0f) / 500.0f, mavlink_mode, 0); } @@ -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..60885aa0a --- /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 = 2048 + +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..bf4a1b579 --- /dev/null +++ b/apps/systemcmds/pwm/pwm.c @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * 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 <alt_channel_mask>] [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" + " <alt_channel_mask> Bitmask of channels to update at the alternate rate\n" + " arm | disarm Arm or disarm the ouptut\n" + " <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"); +} + +int +pwm_main(int argc, char *argv[]) +{ + const char *dev = PWM_OUTPUT_DEVICE_PATH; + unsigned alt_rate = 0; + uint32_t alt_channels; + bool alt_channels_set = false; + bool print_info = false; + int ch; + int ret; + char *ep; + + while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) { + switch (ch) { + case 'c': + alt_channels = strtol(optarg, &ep, 0); + if (*ep != '\0') + usage("bad alt_channel_mask value"); + 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"); + + 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 channels */ + if (alt_channels_set) { + ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, alt_channels); + if (ret != OK) + err(1, "PWM_SERVO_SELECT_UPDATE_RATE (check mask vs. device capabilities)"); + } + + /* 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 */ + printf("PWM output values:\n"); + 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("%u: %uus\n", i, spos); + } else { + printf("%u: ERROR\n", i); + } + } + + /* print rate groups */ + printf("Available alt_channel_mask groups:\n"); + 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(" 0x%x", group_mask); + } + 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 */ |