fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 04:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into new_state_machine

Browse Source 
Conflicts:
	apps/controllib/fixedwing.cpp
This commit is contained in:
Julian Oes
2013-03-22 08:57:26 -07:00
parent 513e1c7398 bee896786a
commit 7f32c01a1d
37 changed files with 1816 additions and 831 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+1 -1
View File
@@ -120,7 +120,7 @@ SERIAL_PORTS = "\\\\.\\COM32,\\\\.\\COM31,\\\\.\\COM30,\\\\.\\COM29,\\\\.\\COM
endif
upload: $(FIRMWARE_BUNDLE) $(UPLOADER)
@python -u $(UPLOADER) --port $(SERIAL_PORTS) $(FIRMWARE_BUNDLE)
$(UPLOADER) --port $(SERIAL_PORTS) $(FIRMWARE_BUNDLE)
#
# JTAG firmware uploading with OpenOCD
ROMFS/Makefile
+1 -1
View File
@@ -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...
ROMFS/mixers/FMU_Q.mix
+2 -2
View File
@@ -26,12 +26,12 @@ 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 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 1 -8000 -8000 0 -10000 10000
Output 2
--------
ROMFS/mixers/FMU_X5.mix
+4 -4
View File
@@ -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
--------
Tools/px_mkfw.py
+1 -1
View File
@@ -107,4 +107,4 @@ if args.image != None:
desc['image_size'] = len(bytes)
desc['image'] = base64.b64encode(zlib.compress(bytes,9))
print json.dumps(desc, indent=4)
print(json.dumps(desc, indent=4))
Tools/px_uploader.py
+8 -8
View File
@@ -262,8 +262,8 @@ class uploader(object):
self.port.flush()
programmed = self.__recv(len(data))
if programmed != data:
print("got " + binascii.hexlify(programmed))
print("expect " + binascii.hexlify(data))
print(("got " + binascii.hexlify(programmed)))
print(("expect " + binascii.hexlify(data)))
return False
self.__getSync()
return True
@@ -307,8 +307,8 @@ class uploader(object):
report_crc = self.__recv_int()
self.__getSync()
if report_crc != expect_crc:
print("Expected 0x%x" % expect_crc)
print("Got 0x%x" % report_crc)
print(("Expected 0x%x" % expect_crc))
print(("Got 0x%x" % report_crc))
raise RuntimeError("Program CRC failed")
# get basic data about the board
@@ -319,7 +319,7 @@ class uploader(object):
# get the bootloader protocol ID first
self.bl_rev = self.__getInfo(uploader.INFO_BL_REV)
if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX):
print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV)
print(("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV))
raise RuntimeError("Bootloader protocol mismatch")
self.board_type = self.__getInfo(uploader.INFO_BOARD_ID)
@@ -360,7 +360,7 @@ args = parser.parse_args()
# Load the firmware file
fw = firmware(args.firmware)
print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))
print(("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))))
# Spin waiting for a device to show up
while True:
@@ -393,7 +393,7 @@ while True:
try:
# identify the bootloader
up.identify()
print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
print(("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)))
except:
# most probably a timeout talking to the port, no bootloader
@@ -406,7 +406,7 @@ while True:
except RuntimeError as ex:
# print the error
print("ERROR: %s" % ex.args)
print(("ERROR: %s" % ex.args))
finally:
# always close the port
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+33 -33
View File
@@ -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);
apps/commander/commander.c
+5 -5
View File
@@ -259,11 +259,11 @@ static int led_off(int led)
}
enum AUDIO_PATTERN {
AUDIO_PATTERN_ERROR = 1,
AUDIO_PATTERN_NOTIFY_POSITIVE = 2,
AUDIO_PATTERN_NOTIFY_NEUTRAL = 3,
AUDIO_PATTERN_NOTIFY_NEGATIVE = 4,
AUDIO_PATTERN_TETRIS = 5
AUDIO_PATTERN_ERROR = 2,
AUDIO_PATTERN_NOTIFY_POSITIVE = 3,
AUDIO_PATTERN_NOTIFY_NEUTRAL = 4,
AUDIO_PATTERN_NOTIFY_NEGATIVE = 5,
AUDIO_PATTERN_NOTIFY_CHARGE = 6
};
int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state)
apps/controllib/block/BlockParam.cpp
+4 -2
View File
@@ -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);
apps/controllib/block/BlockParam.hpp
+8 -3
View File
@@ -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();
}
apps/controllib/fixedwing.cpp
+15 -15
View File
@@ -172,10 +172,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) */
{
}
@@ -326,7 +326,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();
@@ -351,17 +351,17 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
#warning please check whether this flag makes sense
} else if (_status.flag_control_attitude_enabled) {
// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
//
// _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
// _att.rollspeed, _att.pitchspeed, _att.yawspeed);
//
// _actuators.control[CH_AIL] = _stabilization.getAileron();
// _actuators.control[CH_ELV] = - _stabilization.getElevator();
// _actuators.control[CH_RDR] = _stabilization.getRudder();
// _actuators.control[CH_THR] = _manual.throttle;
// }
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
}
}
// update all publications
apps/drivers/blinkm/blinkm.cpp
+4 -4
View File
@@ -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) {
@@ -828,7 +828,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() {
apps/drivers/boards/px4fmu/px4fmu_spi.c
+7 -7
View File
@@ -92,21 +92,21 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
case PX4_SPIDEV_GYRO:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
break;
case PX4_SPIDEV_ACCEL:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
break;
case PX4_SPIDEV_MPU:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
break;
@@ -125,7 +125,7 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
{
/* there can only be one device on this bus, so always select it */
stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 0);
stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected);
}
__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
apps/drivers/drv_pwm_output.h
+32 -15
View File
@@ -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,13 +155,32 @@ __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.
*/
__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.
*
apps/drivers/hil/hil.cpp
+39 -47
View File
@@ -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;
}
apps/drivers/ms5611/ms5611.cpp
+2 -2
View File
@@ -595,7 +595,7 @@ MS5611::cycle()
* spam the console with the message.
*/
} else {
log("collection error %d", ret);
//log("collection error %d", ret);
}
/* reset the collection state machine and try again */
start_cycle();
@@ -627,7 +627,7 @@ MS5611::cycle()
/* measurement phase */
ret = measure();
if (ret != OK) {
log("measure error %d", ret);
//log("measure error %d", ret);
/* reset the collection state machine and try again */
start_cycle();
return;
apps/drivers/px4fmu/fmu.cpp
+138 -88
View File
@@ -90,14 +90,18 @@ public:
virtual int init();
int set_mode(Mode mode);
int set_pwm_rate(unsigned rate);
int set_pwm_alt_rate(unsigned rate);
int set_pwm_alt_channels(uint32_t channels);
private:
static const unsigned _max_actuators = 4;
Mode _mode;
int _update_rate;
int _current_update_rate;
unsigned _pwm_default_rate;
unsigned _pwm_alt_rate;
uint32_t _pwm_alt_rate_channels;
unsigned _current_update_rate;
int _task;
int _t_actuators;
int _t_armed;
@@ -121,6 +125,7 @@ private:
uint8_t control_index,
float &input);
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
struct GPIOConfig {
@@ -163,7 +168,10 @@ PX4FMU *g_fmu;
PX4FMU::PX4FMU() :
CDev("fmuservo", "/dev/px4fmu"),
_mode(MODE_NONE),
_update_rate(50),
_pwm_default_rate(50),
_pwm_alt_rate(50),
_pwm_alt_rate_channels(0),
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
_t_armed(-1),
@@ -263,27 +271,31 @@ PX4FMU::set_mode(Mode mode)
* are presented on the output pins.
*/
switch (mode) {
case MODE_2PWM:
debug("MODE_2PWM");
/* multi-port with flow control lines as PWM */
/* XXX magic numbers */
up_pwm_servo_init(0x3);
_update_rate = 50; /* default output rate */
break;
case MODE_2PWM: // multi-port with flow control lines as PWM
case MODE_4PWM: // multi-port as 4 PWM outs
debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
_pwm_alt_rate_channels = 0;
case MODE_4PWM:
debug("MODE_4PWM");
/* multi-port as 4 PWM outs */
/* XXX magic numbers */
up_pwm_servo_init(0xf);
_update_rate = 50; /* default output rate */
up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
break;
case MODE_NONE:
debug("MODE_NONE");
/* disable servo outputs and set a very low update rate */
_pwm_default_rate = 10; /* artificially reduced output rate */
_pwm_alt_rate = 10;
_pwm_alt_rate_channels = 0;
/* disable servo outputs - no need to set rates */
up_pwm_servo_deinit();
_update_rate = 10;
break;
default:
@@ -295,15 +307,63 @@ PX4FMU::set_mode(Mode mode)
}
int
PX4FMU::set_pwm_rate(unsigned rate)
PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate)
{
if ((rate > 500) || (rate < 10))
return -EINVAL;
debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate);
for (unsigned pass = 0; pass < 2; pass++) {
for (unsigned group = 0; group < _max_actuators; group++) {
// get the channel mask for this rate group
uint32_t mask = up_pwm_servo_get_rate_group(group);
if (mask == 0)
continue;
// all channels in the group must be either default or alt-rate
uint32_t alt = rate_map & mask;
if (pass == 0) {
// preflight
if ((alt != 0) && (alt != mask)) {
warn("rate group %u mask %x bad overlap %x", group, mask, alt);
// not a legal map, bail
return -EINVAL;
}
} else {
// set it - errors here are unexpected
if (alt != 0) {
if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) {
warn("rate group set alt failed");
return -EINVAL;
}
} else {
if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
warn("rate group set default failed");
return -EINVAL;
}
}
}
}
}
_pwm_alt_rate_channels = rate_map;
_pwm_default_rate = default_rate;
_pwm_alt_rate = alt_rate;
_update_rate = rate;
return OK;
}
int
PX4FMU::set_pwm_alt_rate(unsigned rate)
{
return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate);
}
int
PX4FMU::set_pwm_alt_channels(uint32_t channels)
{
return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate);
}
void
PX4FMU::task_main()
{
@@ -353,24 +413,31 @@ PX4FMU::task_main()
/* loop until killed */
while (!_task_should_exit) {
/* handle update rate changes */
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
/*
* Adjust actuator topic update rate to keep up with
* the highest servo update rate configured.
*
* We always mix at max rate; some channels may update slower.
*/
unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
if (_current_update_rate != max_rate) {
_current_update_rate = max_rate;
int update_rate_in_ms = int(1000 / _current_update_rate);
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
_update_rate = 500;
}
/* reject slower than 50 Hz updates */
if (update_rate_in_ms > 20) {
update_rate_in_ms = 20;
_update_rate = 50;
/* reject slower than 10 Hz updates */
if (update_rate_in_ms > 100) {
update_rate_in_ms = 100;
}
debug("adjusted actuator update interval to %ums", update_rate_in_ms);
orb_set_interval(_t_actuators, update_rate_in_ms);
up_pwm_servo_set_rate(_update_rate);
_current_update_rate = _update_rate;
// set to current max rate, even if we are actually checking slower/faster
_current_update_rate = max_rate;
}
/* sleep waiting for data, stopping to check for PPM
@@ -527,7 +594,6 @@ int
PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
lock();
@@ -541,7 +607,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_UPDATE_RATE:
set_pwm_rate(arg);
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
break;
case PWM_SERVO_SELECT_UPDATE_RATE:
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
break;
case PWM_SERVO_SET(2):
@@ -555,9 +625,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET(0):
case PWM_SERVO_SET(1):
if (arg < 2100) {
channel = cmd - PWM_SERVO_SET(0);
up_pwm_servo_set(channel, arg);
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else {
ret = -EINVAL;
}
@@ -573,12 +641,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
channel = cmd - PWM_SERVO_GET(0);
*(servo_position_t *)arg = up_pwm_servo_get(channel);
break;
}
case PWM_SERVO_GET(1):
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
break;
case PWM_SERVO_GET_RATEGROUP(0):
case PWM_SERVO_GET_RATEGROUP(1):
case PWM_SERVO_GET_RATEGROUP(2):
case PWM_SERVO_GET_RATEGROUP(3):
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
@@ -812,7 +886,7 @@ enum PortMode {
PortMode g_port_mode;
int
fmu_new_mode(PortMode new_mode, int update_rate)
fmu_new_mode(PortMode new_mode)
{
uint32_t gpio_bits;
PX4FMU::Mode servo_mode;
@@ -864,9 +938,6 @@ fmu_new_mode(PortMode new_mode, int update_rate)
/* (re)set the PWM output mode */
g_fmu->set_mode(servo_mode);
if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0))
g_fmu->set_pwm_rate(update_rate);
return OK;
}
@@ -962,57 +1033,31 @@ int
fmu_main(int argc, char *argv[])
{
PortMode new_mode = PORT_MODE_UNSET;
int pwm_update_rate_in_hz = 0;
if (!strcmp(argv[1], "test"))
test();
if (!strcmp(argv[1], "fake"))
fake(argc - 1, argv + 1);
const char *verb = argv[1];
if (fmu_start() != OK)
errx(1, "failed to start the FMU driver");
/*
* Mode switches.
*
* XXX use getopt?
*/
for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */
if (!strcmp(argv[i], "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
if (!strcmp(verb, "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
} else if (!strcmp(argv[i], "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
} else if (!strcmp(verb, "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
} else if (!strcmp(argv[i], "mode_pwm")) {
new_mode = PORT_FULL_PWM;
} else if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT_FULL_PWM;
} else if (!strcmp(argv[i], "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
} else if (!strcmp(verb, "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
} else if (!strcmp(argv[i], "mode_pwm_serial")) {
new_mode = PORT_PWM_AND_SERIAL;
} else if (!strcmp(verb, "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 {
errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio");
}
}
} else if (!strcmp(verb, "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
}
/* was a new mode set? */
@@ -1023,12 +1068,17 @@ fmu_main(int argc, char *argv[])
return OK;
/* switch modes */
return fmu_new_mode(new_mode, pwm_update_rate_in_hz);
int ret = fmu_new_mode(new_mode);
exit(ret == OK ? 0 : 1);
}
/* test, etc. here */
if (!strcmp(verb, "test"))
test();
if (!strcmp(verb, "fake"))
fake(argc - 1, argv + 1);
fprintf(stderr, "FMU: unrecognised command, try:\n");
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
exit(1);
}
apps/drivers/px4io/px4io.cpp
+123 -55
View File
@@ -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:
@@ -1224,14 +1237,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));
@@ -1270,10 +1284,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),
@@ -1321,36 +1335,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);
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_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);
/* 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_SELECT_UPDATE_RATE: {
/* 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;
@@ -1362,7 +1379,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);
@@ -1380,6 +1397,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;
@@ -1444,6 +1471,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;
@@ -1467,11 +1508,50 @@ 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)
{
@@ -1566,32 +1646,20 @@ px4io_main(int argc, char *argv[])
if (argc < 2)
goto out;
if (!strcmp(argv[1], "start")) {
if (!strcmp(argv[1], "start"))
start(argc - 1, argv + 1);
if (g_dev != nullptr)
errx(1, "already loaded");
if (!strcmp(argv[1], "limit")) {
/* create the driver - it will set g_dev */
(void)new PX4IO();
if (g_dev != nullptr) {
if (g_dev == nullptr)
errx(1, "driver alloc failed");
if (OK != g_dev->init()) {
delete g_dev;
errx(1, "driver init failed");
}
/* 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);
}
@@ -1603,7 +1671,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");
}
@@ -1624,16 +1692,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");
@@ -1647,7 +1715,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);
@@ -1722,5 +1790,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'");
}
apps/drivers/stm32/drv_pwm_servo.c
+34 -15
View File
@@ -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)
{
apps/drivers/stm32/tone_alarm/tone_alarm.cpp
+668 -302
View File
File diff suppressed because it is too large Load Diff
apps/examples/control_demo/params.c
-4
View File
@@ -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)
apps/mavlink/orb_listener.c
+48 -48
View File
@@ -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);
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);
struct listener listeners[] = {
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;
apps/px4/tests/test_bson.c
+1 -1
View File
@@ -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
apps/px4io/controls.c
+5 -1
View File
@@ -177,7 +177,7 @@ controls_tick() {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
} else {
/* in the configured dead zone, output zero */
@@ -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);
}
apps/px4io/mixer.cpp
+5 -4
View File
@@ -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 */
apps/px4io/protocol.h
+14 -9
View File
@@ -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) */
apps/px4io/px4io.h
+2 -2
View File
@@ -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])
apps/px4io/registers.c
+65 -16
View File
@@ -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;
}
apps/sensors/sensors.cpp
+1 -1
View File
@@ -1142,7 +1142,7 @@ Sensors::ppm_poll()
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
} else {
/* in the configured dead zone, output zero */
apps/systemcmds/preflight_check/preflight_check.c
+8
View File
@@ -54,6 +54,8 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_baro.h>
#include <mavlink/mavlink_log.h>
__EXPORT int preflight_check_main(int argc, char *argv[]);
static int led_toggle(int leds, int led);
static int led_on(int leds, int led);
@@ -75,6 +77,8 @@ int preflight_check_main(int argc, char *argv[])
bool system_ok = true;
int fd;
/* open text message output path */
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
int ret;
/* give the system some time to sample the sensors in the background */
@@ -86,6 +90,7 @@ int preflight_check_main(int argc, char *argv[])
fd = open(MAG_DEVICE_PATH, 0);
if (fd < 0) {
warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG");
system_ok = false;
goto system_eval;
}
@@ -93,6 +98,7 @@ int preflight_check_main(int argc, char *argv[])
if (ret != OK) {
warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CALIBRATION");
system_ok = false;
goto system_eval;
}
@@ -105,6 +111,7 @@ int preflight_check_main(int argc, char *argv[])
if (ret != OK) {
warnx("accel self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK");
system_ok = false;
goto system_eval;
}
@@ -117,6 +124,7 @@ int preflight_check_main(int argc, char *argv[])
if (ret != OK) {
warnx("gyro self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK");
system_ok = false;
goto system_eval;
}
apps/systemcmds/pwm/Makefile
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Build the pwm tool.
#
APPNAME = pwm
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 4096
include $(APPDIR)/mk/app.mk
apps/systemcmds/pwm/pwm.c
+233
View File
@@ -0,0 +1,233 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file pwm.c
*
* PWM servo output configuration and monitoring tool.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <nuttx/i2c.h>
#include <nuttx/mtd.h>
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/err.h"
#include "drivers/drv_pwm_output.h"
static void usage(const char *reason);
__EXPORT int pwm_main(int argc, char *argv[]);
static void
usage(const char *reason)
{
if (reason != NULL)
warnx("%s", reason);
errx(1,
"usage:\n"
"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [arm|disarm] [<channel_value> ...]\n"
" -v Print information about the PWM device\n"
" <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
" <alt_rate> PWM update rate for channels in <alt_channel_mask>\n"
" <channel_group> Channel group that should update at the alternate rate (may be specified more than once)\n"
" arm | disarm Arm or disarm the ouptut\n"
" <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"
"\n"
"When -c is specified, any channel groups not listed with -c will update at the default rate.\n"
);
}
int
pwm_main(int argc, char *argv[])
{
const char *dev = PWM_OUTPUT_DEVICE_PATH;
unsigned alt_rate = 0;
uint32_t alt_channel_groups = 0;
bool alt_channels_set = false;
bool print_info = false;
int ch;
int ret;
char *ep;
unsigned group;
if (argc < 2)
usage(NULL);
while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) {
switch (ch) {
case 'c':
group = strtoul(optarg, &ep, 0);
if ((*ep != '\0') || (group >= 32))
usage("bad channel_group value");
alt_channel_groups |= (1 << group);
alt_channels_set = true;
break;
case 'd':
dev = optarg;
break;
case 'u':
alt_rate = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alt_rate value");
break;
case 'v':
print_info = true;
break;
default:
usage(NULL);
}
}
argc -= optind;
argv += optind;
/* open for ioctl only */
int fd = open(dev, 0);
if (fd < 0)
err(1, "can't open %s", dev);
/* change alternate PWM rate */
if (alt_rate > 0) {
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
if (ret != OK)
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
}
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
for (unsigned group = 0; group < 32; group++) {
if ((1 << group) & alt_channel_groups) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
mask |= group_mask;
}
}
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
}
/* iterate remaining arguments */
unsigned channel = 0;
while (argc--) {
const char *arg = argv[0];
argv++;
if (!strcmp(arg, "arm")) {
ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
continue;
}
if (!strcmp(arg, "disarm")) {
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
continue;
}
unsigned pwm_value = strtol(arg, &ep, 0);
if (*ep == '\0') {
ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", channel);
channel++;
continue;
}
usage("unrecognised option");
}
/* print verbose info */
if (print_info) {
/* get the number of servo channels */
unsigned count;
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
if (ret != OK)
err(1, "PWM_SERVO_GET_COUNT");
/* print current servo values */
for (unsigned i = 0; i < count; i++) {
servo_position_t spos;
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
if (ret == OK) {
printf("channel %u: %uus\n", i, spos);
} else {
printf("%u: ERROR\n", i);
}
}
/* print rate groups */
for (unsigned i = 0; i < count; i++) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
if (ret != OK)
break;
if (group_mask != 0) {
printf("channel group %u: channels", i);
for (unsigned j = 0; j < 32; j++)
if (group_mask & (1 << j))
printf(" %u", j);
printf("\n");
}
}
fflush(stdout);
}
exit(0);
}
apps/systemlib/param/param.c
+1 -1
View File
@@ -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);
nuttx/arch/arm/src/stm32/stm32_otgfsdev.c
+119 -41
View File
@@ -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;
nuttx/arch/arm/src/stm32/stm32_usbdev.c
+113 -70
View File
@@ -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()
nuttx/configs/px4fmu/nsh/appconfig
+1
View File
@@ -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
nuttx/libc/stdio/lib_sscanf.c
+29 -23
View File
@@ -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 */