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

tap_esc: some cleanup 

- use select_responder from tap_esc_common namespace (same definition)
- switch pwm output to uint16, that's what's used in send_esc_output()
- use SCHED_PRIORITY_ACTUATOR_OUTPUTS instead of SCHED_PRIORITY_MAX
This commit is contained in:
Beat Küng 2017-10-06 19:05:21 +02:00 committed by Lorenz Meier
parent 38f45d1a9d
commit efb170d91d
1 changed files with 19 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
src/drivers/tap_esc/tap_esc.cpp
View File

@ -102,8 +102,7 @@ public:
virtual int init(); virtual int init();
virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual int ioctl(file *filp, int cmd, unsigned long arg);
void cycle(); void cycle();
protected:
void select_responder(uint8_t sel);
private: private:
static const uint8_t device_mux_map[TAP_ESC_MAX_MOTOR_NUM]; static const uint8_t device_mux_map[TAP_ESC_MAX_MOTOR_NUM];
@ -147,7 +146,7 @@ private:
void work_start(); void work_start();
void work_stop(); void work_stop();
void send_esc_outputs(const float *pwm, const unsigned num_pwm); void send_esc_outputs(const uint16_t *pwm, const unsigned num_pwm);
uint8_t crc8_esc(uint8_t *p, uint8_t len); uint8_t crc8_esc(uint8_t *p, uint8_t len);
uint8_t crc_packet(EscPacket &p); uint8_t crc_packet(EscPacket &p);
int send_packet(EscPacket &p, int responder); int send_packet(EscPacket &p, int responder);
@ -358,7 +357,7 @@ int TAP_ESC::send_packet(EscPacket &packet, int responder)
return -EINVAL; return -EINVAL;
} }
select_responder(responder); tap_esc_common::select_responder(responder);
} }
int packet_len = crc_packet(packet); int packet_len = crc_packet(packet);
@ -416,17 +415,8 @@ uint8_t TAP_ESC::crc_packet(EscPacket &p)
p.d.bytes[p.len] = crc8_esc(&p.len, p.len + 2); p.d.bytes[p.len] = crc8_esc(&p.len, p.len + 2);
return p.len + offsetof(EscPacket, d) + 1; return p.len + offsetof(EscPacket, d) + 1;
} }
void TAP_ESC::select_responder(uint8_t sel)
{
#if defined(GPIO_S0)
px4_arch_gpiowrite(GPIO_S0, sel & 1);
px4_arch_gpiowrite(GPIO_S1, sel & 2);
px4_arch_gpiowrite(GPIO_S2, sel & 4);
#endif
}
void TAP_ESC::send_esc_outputs(const uint16_t *pwm, const unsigned num_pwm)
void TAP_ESC:: send_esc_outputs(const float *pwm, const unsigned num_pwm)
{ {
uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM]; uint16_t rpm[TAP_ESC_MAX_MOTOR_NUM];
@ -721,30 +711,30 @@ TAP_ESC::cycle()
} }
const unsigned esc_count = num_outputs; const unsigned esc_count = num_outputs;
float motor_out[TAP_ESC_MAX_MOTOR_NUM]; uint16_t motor_out[TAP_ESC_MAX_MOTOR_NUM];
// We need to remap from the system default to what PX4's normal // We need to remap from the system default to what PX4's normal
// scheme is // scheme is
if (num_outputs == 6) { if (num_outputs == 6) {
motor_out[0] = _outputs.output[3]; motor_out[0] = (uint16_t)_outputs.output[3];
motor_out[1] = _outputs.output[0]; motor_out[1] = (uint16_t)_outputs.output[0];
motor_out[2] = _outputs.output[4]; motor_out[2] = (uint16_t)_outputs.output[4];
motor_out[3] = _outputs.output[2]; motor_out[3] = (uint16_t)_outputs.output[2];
motor_out[4] = _outputs.output[1]; motor_out[4] = (uint16_t)_outputs.output[1];
motor_out[5] = _outputs.output[5]; motor_out[5] = (uint16_t)_outputs.output[5];
motor_out[6] = RPMSTOPPED; motor_out[6] = RPMSTOPPED;
motor_out[7] = RPMSTOPPED; motor_out[7] = RPMSTOPPED;
} else if (num_outputs == 4) { } else if (num_outputs == 4) {
motor_out[0] = _outputs.output[2]; motor_out[0] = (uint16_t)_outputs.output[2];
motor_out[2] = _outputs.output[0]; motor_out[2] = (uint16_t)_outputs.output[0];
motor_out[1] = _outputs.output[1]; motor_out[1] = (uint16_t)_outputs.output[1];
motor_out[3] = _outputs.output[3]; motor_out[3] = (uint16_t)_outputs.output[3];
} else { } else {
// Use the system defaults // Use the system defaults
for (unsigned i = 0; i < esc_count; ++i) { for (unsigned i = 0; i < esc_count; ++i) {
motor_out[i] = _outputs.output[i]; motor_out[i] = (uint16_t)_outputs.output[i];
} }
} }
@ -1099,9 +1089,9 @@ void start()
_task_should_exit = false; _task_should_exit = false;
/* start the task */ /* start the task */
_task_handle = px4_task_spawn_cmd("tap_esc_main", _task_handle = px4_task_spawn_cmd("tap_esc",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX, SCHED_PRIORITY_ACTUATOR_OUTPUTS,
1100, 1100,
(px4_main_t)&task_main_trampoline, (px4_main_t)&task_main_trampoline,
nullptr); nullptr);