forked from Archive/PX4-Autopilot
Added sensor rail reset IOCTL and command (fmu sensor_reset 10 resets for 10 ms)
This commit is contained in:
parent
881cf61553
commit
acc3cc087f
|
@ -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 */
|
|
@ -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);
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -952,12 +1007,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
||||||
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,11 +1444,27 @@ 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;
|
||||||
|
@ -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);
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue