dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Added sensor rail reset IOCTL and command (fmu sensor_reset 10 resets for 10 ms)

This commit is contained in:
Lorenz Meier 2013-12-04 08:17:35 +01:00
parent 881cf61553
commit acc3cc087f
2 changed files with 280 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/drivers/drv_gpio.h
View File

@ -46,7 +46,7 @@
/* /*
* PX4FMU GPIO numbers. * PX4FMU GPIO numbers.
* *
* For shared pins, alternate function 1 selects the non-GPIO mode * For shared pins, alternate function 1 selects the non-GPIO mode
* (USART2, CAN2, etc.) * (USART2, CAN2, etc.)
*/ */
# define GPIO_EXT_1 (1<<0) /**< high-power GPIO 1 */ # define GPIO_EXT_1 (1<<0) /**< high-power GPIO 1 */
@ -144,4 +144,6 @@
/** read all the GPIOs and return their values in *(uint32_t *)arg */ /** read all the GPIOs and return their values in *(uint32_t *)arg */
#define GPIO_GET GPIOC(12) #define GPIO_GET GPIOC(12)
#define GPIO_SENSOR_RAIL_RESET GPIOC(13)
#endif /* _DRV_GPIO_H */ #endif /* _DRV_GPIO_H */

456
src/drivers/px4fmu/fmu.cpp
View File

@ -164,6 +164,7 @@ private:
static const unsigned _ngpio; static const unsigned _ngpio;
void gpio_reset(void); void gpio_reset(void);
void sensor_reset(int ms);
void gpio_set_function(uint32_t gpios, int function); void gpio_set_function(uint32_t gpios, int function);
void gpio_write(uint32_t gpios, int function); void gpio_write(uint32_t gpios, int function);
uint32_t gpio_read(void); uint32_t gpio_read(void);
@ -226,10 +227,10 @@ PX4FMU::PX4FMU() :
_armed(false), _armed(false),
_pwm_on(false), _pwm_on(false),
_mixers(nullptr), _mixers(nullptr),
_failsafe_pwm({0}), _failsafe_pwm( {0}),
_disarmed_pwm({0}), _disarmed_pwm( {0}),
_num_failsafe_set(0), _num_failsafe_set(0),
_num_disarmed_set(0) _num_disarmed_set(0)
{ {
for (unsigned i = 0; i < _max_actuators; i++) { for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN; _min_pwm[i] = PWM_DEFAULT_MIN;
@ -293,11 +294,11 @@ PX4FMU::init()
/* start the IO interface task */ /* start the IO interface task */
_task = task_spawn_cmd("fmuservo", _task = task_spawn_cmd("fmuservo",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT, SCHED_PRIORITY_DEFAULT,
2048, 2048,
(main_t)&PX4FMU::task_main_trampoline, (main_t)&PX4FMU::task_main_trampoline,
nullptr); nullptr);
if (_task < 0) { if (_task < 0) {
debug("task start failed: %d", errno); debug("task start failed: %d", errno);
@ -326,7 +327,7 @@ PX4FMU::set_mode(Mode mode)
switch (mode) { switch (mode) {
case MODE_2PWM: // v1 multi-port with flow control lines as PWM case MODE_2PWM: // v1 multi-port with flow control lines as PWM
debug("MODE_2PWM"); debug("MODE_2PWM");
/* default output rates */ /* default output rates */
_pwm_default_rate = 50; _pwm_default_rate = 50;
_pwm_alt_rate = 50; _pwm_alt_rate = 50;
@ -340,7 +341,7 @@ PX4FMU::set_mode(Mode mode)
case MODE_4PWM: // v1 multi-port as 4 PWM outs case MODE_4PWM: // v1 multi-port as 4 PWM outs
debug("MODE_4PWM"); debug("MODE_4PWM");
/* default output rates */ /* default output rates */
_pwm_default_rate = 50; _pwm_default_rate = 50;
_pwm_alt_rate = 50; _pwm_alt_rate = 50;
@ -354,7 +355,7 @@ PX4FMU::set_mode(Mode mode)
case MODE_6PWM: // v2 PWMs as 6 PWM outs case MODE_6PWM: // v2 PWMs as 6 PWM outs
debug("MODE_6PWM"); debug("MODE_6PWM");
/* default output rates */ /* default output rates */
_pwm_default_rate = 50; _pwm_default_rate = 50;
_pwm_alt_rate = 50; _pwm_alt_rate = 50;
@ -396,6 +397,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
// get the channel mask for this rate group // get the channel mask for this rate group
uint32_t mask = up_pwm_servo_get_rate_group(group); uint32_t mask = up_pwm_servo_get_rate_group(group);
if (mask == 0) if (mask == 0)
continue; continue;
@ -409,6 +411,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
// not a legal map, bail // not a legal map, bail
return -EINVAL; return -EINVAL;
} }
} else { } else {
// set it - errors here are unexpected // set it - errors here are unexpected
if (alt != 0) { if (alt != 0) {
@ -416,6 +419,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
warn("rate group set alt failed"); warn("rate group set alt failed");
return -EINVAL; return -EINVAL;
} }
} else { } else {
if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) { if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
warn("rate group set default failed"); warn("rate group set default failed");
@ -425,6 +429,7 @@ PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate
} }
} }
} }
_pwm_alt_rate_channels = rate_map; _pwm_alt_rate_channels = rate_map;
_pwm_default_rate = default_rate; _pwm_default_rate = default_rate;
_pwm_alt_rate = alt_rate; _pwm_alt_rate = alt_rate;
@ -471,7 +476,7 @@ PX4FMU::task_main()
memset(&controls_effective, 0, sizeof(controls_effective)); memset(&controls_effective, 0, sizeof(controls_effective));
/* advertise the effective control inputs */ /* advertise the effective control inputs */
_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
&controls_effective); &controls_effective);
pollfd fds[2]; pollfd fds[2];
fds[0].fd = _t_actuators; fds[0].fd = _t_actuators;
@ -503,6 +508,7 @@ PX4FMU::task_main()
* We always mix at max rate; some channels may update slower. * 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; unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
if (_current_update_rate != max_rate) { if (_current_update_rate != max_rate) {
_current_update_rate = max_rate; _current_update_rate = max_rate;
int update_rate_in_ms = int(1000 / _current_update_rate); int update_rate_in_ms = int(1000 / _current_update_rate);
@ -511,6 +517,7 @@ PX4FMU::task_main()
if (update_rate_in_ms < 2) { if (update_rate_in_ms < 2) {
update_rate_in_ms = 2; update_rate_in_ms = 2;
} }
/* reject slower than 10 Hz updates */ /* reject slower than 10 Hz updates */
if (update_rate_in_ms > 100) { if (update_rate_in_ms > 100) {
update_rate_in_ms = 100; update_rate_in_ms = 100;
@ -532,6 +539,7 @@ PX4FMU::task_main()
log("poll error %d", errno); log("poll error %d", errno);
usleep(1000000); usleep(1000000);
continue; continue;
} else if (ret == 0) { } else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */ /* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe"); // warnx("no PWM: failsafe");
@ -553,12 +561,15 @@ PX4FMU::task_main()
case MODE_2PWM: case MODE_2PWM:
num_outputs = 2; num_outputs = 2;
break; break;
case MODE_4PWM: case MODE_4PWM:
num_outputs = 4; num_outputs = 4;
break; break;
case MODE_6PWM: case MODE_6PWM:
num_outputs = 6; num_outputs = 6;
break; break;
default: default:
num_outputs = 0; num_outputs = 0;
break; break;
@ -572,9 +583,9 @@ PX4FMU::task_main()
for (unsigned i = 0; i < num_outputs; i++) { for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */ /* last resort: catch NaN, INF and out-of-band errors */
if (i >= outputs.noutputs || if (i >= outputs.noutputs ||
!isfinite(outputs.output[i]) || !isfinite(outputs.output[i]) ||
outputs.output[i] < -1.0f || outputs.output[i] < -1.0f ||
outputs.output[i] > 1.0f) { outputs.output[i] > 1.0f) {
/* /*
* Value is NaN, INF or out of band - set to the minimum value. * Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally * This will be clearly visible on the servo status and will limit the risk of accidentally
@ -592,6 +603,7 @@ PX4FMU::task_main()
for (unsigned i = 0; i < num_outputs; i++) { for (unsigned i = 0; i < num_outputs; i++) {
controls_effective.control_effective[i] = (float)pwm_limited[i]; controls_effective.control_effective[i] = (float)pwm_limited[i];
} }
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* output to the servos */ /* output to the servos */
@ -613,11 +625,13 @@ PX4FMU::task_main()
/* update the armed status and check that we're not locked down */ /* update the armed status and check that we're not locked down */
bool set_armed = aa.armed && !aa.lockdown; bool set_armed = aa.armed && !aa.lockdown;
if (_armed != set_armed) if (_armed != set_armed)
_armed = set_armed; _armed = set_armed;
/* update PWM status if armed or if disarmed PWM values are set */ /* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (aa.armed || _num_disarmed_set > 0); bool pwm_on = (aa.armed || _num_disarmed_set > 0);
if (_pwm_on != pwm_on) { if (_pwm_on != pwm_on) {
_pwm_on = pwm_on; _pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on); up_pwm_servo_arm(pwm_on);
@ -626,25 +640,31 @@ PX4FMU::task_main()
} }
#ifdef HRT_PPM_CHANNEL #ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data // see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) { if (ppm_last_valid_decode != rc_in.timestamp) {
// we have a new PPM frame. Publish it. // we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels; rc_in.channel_count = ppm_decoded_channels;
if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = RC_INPUT_MAX_CHANNELS; rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
} }
for (uint8_t i=0; i<rc_in.channel_count; i++) {
for (uint8_t i = 0; i < rc_in.channel_count; i++) {
rc_in.values[i] = ppm_buffer[i]; rc_in.values[i] = ppm_buffer[i];
} }
rc_in.timestamp = ppm_last_valid_decode; rc_in.timestamp = ppm_last_valid_decode;
/* lazily advertise on first publication */ /* lazily advertise on first publication */
if (to_input_rc == 0) { if (to_input_rc == 0) {
to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in); to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
} else {
} else {
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in); orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
} }
} }
#endif #endif
} }
@ -753,142 +773,176 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break; break;
case PWM_SERVO_SET_FAILSAFE_PWM: { case PWM_SERVO_SET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) { /* discard if too many values are sent */
ret = -EINVAL; if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_failsafe_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_failsafe_pwm[i] = PWM_LOWEST_MIN;
} else {
_failsafe_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_failsafe_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_failsafe_pwm[i] > 0)
_num_failsafe_set++;
}
break; break;
} }
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_failsafe_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_failsafe_pwm[i] = PWM_LOWEST_MIN;
} else {
_failsafe_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_failsafe_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_failsafe_pwm[i] > 0)
_num_failsafe_set++;
}
break;
}
case PWM_SERVO_GET_FAILSAFE_PWM: { case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _failsafe_pwm[i]; for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _failsafe_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
} }
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_DISARMED_PWM: { case PWM_SERVO_SET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) { /* discard if too many values are sent */
ret = -EINVAL; if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_disarmed_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_disarmed_pwm[i] = PWM_LOWEST_MIN;
} else {
_disarmed_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_disarmed_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_disarmed_pwm[i] > 0)
_num_disarmed_set++;
}
break; break;
} }
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_disarmed_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_disarmed_pwm[i] = PWM_LOWEST_MIN;
} else {
_disarmed_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_disarmed_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_disarmed_pwm[i] > 0)
_num_disarmed_set++;
}
break;
}
case PWM_SERVO_GET_DISARMED_PWM: { case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _disarmed_pwm[i]; for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _disarmed_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
} }
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_MIN_PWM: { case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) { /* discard if too many values are sent */
ret = -EINVAL; if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_min_pwm[i] = PWM_HIGHEST_MIN;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_min_pwm[i] = PWM_LOWEST_MIN;
} else {
_min_pwm[i] = pwm->values[i];
}
}
break; break;
} }
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_min_pwm[i] = PWM_HIGHEST_MIN;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_min_pwm[i] = PWM_LOWEST_MIN;
} else {
_min_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MIN_PWM: { case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _min_pwm[i]; for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _min_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
} }
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
case PWM_SERVO_SET_MAX_PWM: { case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) { /* discard if too many values are sent */
ret = -EINVAL; if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_max_pwm[i] = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_max_pwm[i] = PWM_HIGHEST_MAX;
} else {
_max_pwm[i] = pwm->values[i];
}
}
break; break;
} }
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_max_pwm[i] = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_max_pwm[i] = PWM_HIGHEST_MAX;
} else {
_max_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MAX_PWM: { case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg; struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _max_pwm[i]; for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _max_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
} }
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
case PWM_SERVO_SET(5): case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4): case PWM_SERVO_SET(4):
@ -910,6 +964,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET(0): case PWM_SERVO_SET(0):
if (arg <= 2100) { if (arg <= 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else { } else {
ret = -EINVAL; ret = -EINVAL;
} }
@ -946,18 +1001,21 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break; break;
case PWM_SERVO_GET_COUNT: case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT: case MIXERIOCGETOUTPUTCOUNT:
switch (_mode) { switch (_mode) {
case MODE_6PWM: case MODE_6PWM:
*(unsigned *)arg = 6; *(unsigned *)arg = 6;
break; break;
case MODE_4PWM: case MODE_4PWM:
*(unsigned *)arg = 4; *(unsigned *)arg = 4;
break; break;
case MODE_2PWM: case MODE_2PWM:
*(unsigned *)arg = 2; *(unsigned *)arg = 2;
break; break;
default: default:
ret = -EINVAL; ret = -EINVAL;
break; break;
@ -1015,6 +1073,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL; ret = -EINVAL;
} }
} }
break; break;
} }
@ -1049,55 +1108,58 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
for (uint8_t i = 0; i < count; i++) { for (uint8_t i = 0; i < count; i++) {
up_pwm_servo_set(i, values[i]); up_pwm_servo_set(i, values[i]);
} }
return count * 2; return count * 2;
} }
void void
PX4FMU::sensor_reset(int ms) PX4FMU::sensor_reset(int ms)
{ {
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
if (ms < 1) { if (ms < 1) {
ms = 1; ms = 1;
} }
/* disable SPI bus */ /* disable SPI bus */
stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
stm32_configgpio(GPIO_SPI_CS_BARO_OFF); stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
stm32_configgpio(GPIO_SPI1_SCK_OFF); stm32_configgpio(GPIO_SPI1_SCK_OFF);
stm32_configgpio(GPIO_SPI1_MISO_OFF); stm32_configgpio(GPIO_SPI1_MISO_OFF);
stm32_configgpio(GPIO_SPI1_MOSI_OFF); stm32_configgpio(GPIO_SPI1_MOSI_OFF);
stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0); stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0); stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0); stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
stm32_configgpio(GPIO_GYRO_DRDY_OFF); stm32_configgpio(GPIO_GYRO_DRDY_OFF);
stm32_configgpio(GPIO_MAG_DRDY_OFF); stm32_configgpio(GPIO_MAG_DRDY_OFF);
stm32_configgpio(GPIO_ACCEL_DRDY_OFF); stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
/* set the sensor rail off */ /* set the sensor rail off */
stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
/* wait for the sensor rail to reach GND */ /* wait for the sensor rail to reach GND */
usleep(ms * 000); usleep(ms * 000);
/* re-enable power */ /* re-enable power */
/* switch the sensor rail back on */ /* switch the sensor rail back on */
stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
/* wait a bit before starting SPI, different times didn't influence results */ /* wait a bit before starting SPI, different times didn't influence results */
usleep(100); usleep(100);
/* reconfigure the SPI pins */ /* reconfigure the SPI pins */
#ifdef CONFIG_STM32_SPI1 #ifdef CONFIG_STM32_SPI1
@ -1115,8 +1177,10 @@ PX4FMU::sensor_reset(int ms)
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
#endif #endif
#endif
} }
void void
PX4FMU::gpio_reset(void) PX4FMU::gpio_reset(void)
{ {
@ -1127,6 +1191,7 @@ PX4FMU::gpio_reset(void)
for (unsigned i = 0; i < _ngpio; i++) { for (unsigned i = 0; i < _ngpio; i++) {
if (_gpio_tab[i].input != 0) { if (_gpio_tab[i].input != 0) {
stm32_configgpio(_gpio_tab[i].input); stm32_configgpio(_gpio_tab[i].input);
} else if (_gpio_tab[i].output != 0) { } else if (_gpio_tab[i].output != 0) {
stm32_configgpio(_gpio_tab[i].output); stm32_configgpio(_gpio_tab[i].output);
} }
@ -1143,6 +1208,7 @@ void
PX4FMU::gpio_set_function(uint32_t gpios, int function) PX4FMU::gpio_set_function(uint32_t gpios, int function)
{ {
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/* /*
* GPIOs 0 and 1 must have the same direction as they are buffered * GPIOs 0 and 1 must have the same direction as they are buffered
* by a shared 2-port driver. Any attempt to set either sets both. * by a shared 2-port driver. Any attempt to set either sets both.
@ -1154,6 +1220,7 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
if (GPIO_SET_OUTPUT == function) if (GPIO_SET_OUTPUT == function)
stm32_gpiowrite(GPIO_GPIO_DIR, 1); stm32_gpiowrite(GPIO_GPIO_DIR, 1);
} }
#endif #endif
/* configure selected GPIOs as required */ /* configure selected GPIOs as required */
@ -1178,9 +1245,11 @@ PX4FMU::gpio_set_function(uint32_t gpios, int function)
} }
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
/* flip buffer to input mode if required */ /* flip buffer to input mode if required */
if ((GPIO_SET_INPUT == function) && (gpios & 3)) if ((GPIO_SET_INPUT == function) && (gpios & 3))
stm32_gpiowrite(GPIO_GPIO_DIR, 0); stm32_gpiowrite(GPIO_GPIO_DIR, 0);
#endif #endif
} }
@ -1219,8 +1288,8 @@ PX4FMU::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
gpio_reset(); gpio_reset();
break; break;
case SENSOR_RESET: case GPIO_SENSOR_RAIL_RESET:
sensor_reset(); sensor_reset(20);
break; break;
case GPIO_SET_OUTPUT: case GPIO_SET_OUTPUT:
@ -1296,8 +1365,9 @@ fmu_new_mode(PortMode new_mode)
#endif #endif
break; break;
/* mixed modes supported on v1 board only */ /* mixed modes supported on v1 board only */
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
case PORT_FULL_SERIAL: case PORT_FULL_SERIAL:
/* set all multi-GPIOs to serial mode */ /* set all multi-GPIOs to serial mode */
gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4; gpio_bits = GPIO_MULTI_1 | GPIO_MULTI_2 | GPIO_MULTI_3 | GPIO_MULTI_4;
@ -1320,6 +1390,7 @@ fmu_new_mode(PortMode new_mode)
servo_mode = PX4FMU::MODE_2PWM; servo_mode = PX4FMU::MODE_2PWM;
break; break;
#endif #endif
default: default:
return -1; return -1;
} }
@ -1373,15 +1444,31 @@ fmu_stop(void)
return ret; return ret;
} }
void
sensor_reset(int ms)
{
int fd;
int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0)
errx(1, "open fail");
if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0)
err(1, "servo arm failed");
}
void void
test(void) test(void)
{ {
int fd; int fd;
unsigned servo_count = 0; unsigned servo_count = 0;
unsigned pwm_value = 1000; unsigned pwm_value = 1000;
int direction = 1; int direction = 1;
int ret; int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR); fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
if (fd < 0) if (fd < 0)
@ -1389,9 +1476,9 @@ test(void)
if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed"); if (ioctl(fd, PWM_SERVO_ARM, 0) < 0) err(1, "servo arm failed");
if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) { if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count) != 0) {
err(1, "Unable to get servo count\n"); err(1, "Unable to get servo count\n");
} }
warnx("Testing %u servos", (unsigned)servo_count); warnx("Testing %u servos", (unsigned)servo_count);
@ -1404,32 +1491,38 @@ test(void)
for (;;) { for (;;) {
/* sweep all servos between 1000..2000 */ /* sweep all servos between 1000..2000 */
servo_position_t servos[servo_count]; servo_position_t servos[servo_count];
for (unsigned i = 0; i < servo_count; i++) for (unsigned i = 0; i < servo_count; i++)
servos[i] = pwm_value; servos[i] = pwm_value;
if (direction == 1) { if (direction == 1) {
// use ioctl interface for one direction // use ioctl interface for one direction
for (unsigned i=0; i < servo_count; i++) { for (unsigned i = 0; i < servo_count; i++) {
if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) { if (ioctl(fd, PWM_SERVO_SET(i), servos[i]) < 0) {
err(1, "servo %u set failed", i); err(1, "servo %u set failed", i);
} }
} }
} else {
// and use write interface for the other direction } else {
ret = write(fd, servos, sizeof(servos)); // and use write interface for the other direction
if (ret != (int)sizeof(servos)) ret = write(fd, servos, sizeof(servos));
err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
} if (ret != (int)sizeof(servos))
err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
}
if (direction > 0) { if (direction > 0) {
if (pwm_value < 2000) { if (pwm_value < 2000) {
pwm_value++; pwm_value++;
} else { } else {
direction = -1; direction = -1;
} }
} else { } else {
if (pwm_value > 1000) { if (pwm_value > 1000) {
pwm_value--; pwm_value--;
} else { } else {
direction = 1; direction = 1;
} }
@ -1441,6 +1534,7 @@ test(void)
if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value)) if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
err(1, "error reading PWM servo %d", i); err(1, "error reading PWM servo %d", i);
if (value != servos[i]) if (value != servos[i])
errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]); errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
} }
@ -1448,12 +1542,14 @@ test(void)
/* Check if user wants to quit */ /* Check if user wants to quit */
char c; char c;
ret = poll(&fds, 1, 0); ret = poll(&fds, 1, 0);
if (ret > 0) { if (ret > 0) {
read(0, &c, 1); read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') { if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n"); warnx("User abort\n");
break; break;
} }
} }
} }
@ -1526,6 +1622,7 @@ fmu_main(int argc, char *argv[])
new_mode = PORT_FULL_PWM; new_mode = PORT_FULL_PWM;
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
} else if (!strcmp(verb, "mode_serial")) { } else if (!strcmp(verb, "mode_serial")) {
new_mode = PORT_FULL_SERIAL; new_mode = PORT_FULL_SERIAL;
@ -1561,6 +1658,7 @@ fmu_main(int argc, char *argv[])
if (!strcmp(verb, "sensor_reset")) if (!strcmp(verb, "sensor_reset"))
if (argc > 2) { if (argc > 2) {
sensor_reset(strtol(argv[2], 0, 0)); sensor_reset(strtol(argv[2], 0, 0));
} else { } else {
sensor_reset(0); sensor_reset(0);
} }