Browse Source

dshot: inline up_dshot_motor_data_set and up_dshot_motor_command

Slight performance improvement (~0.1% @1khz on F4)
master
Beat Küng 4 years ago committed by Daniel Agar
parent
commit
afed10618b
  1. 12
      platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c
  2. 16
      src/drivers/drv_pwm_output.h

12
platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c

@ -200,7 +200,7 @@ void up_dshot_trigger(void) @@ -200,7 +200,7 @@ void up_dshot_trigger(void)
* bit 12 - dshot telemetry enable/disable
* bits 13-16 - XOR checksum
**/
static void dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool telemetry)
void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry)
{
uint16_t packet = 0;
uint16_t checksum = 0;
@ -234,16 +234,6 @@ static void dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool @@ -234,16 +234,6 @@ static void dshot_motor_data_set(uint32_t motor_number, uint16_t throttle, bool
motor_buffer[motor_number * ONE_MOTOR_BUFF_SIZE + DSHOT_END_OF_STREAM] = 0;
}
void up_dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry)
{
dshot_motor_data_set(motor_number, throttle + DShot_cmd_MIN_throttle, telemetry);
}
void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry)
{
dshot_motor_data_set(channel, command, telemetry);
}
void dshot_dmar_data_prepare(uint8_t timer, uint8_t first_motor, uint8_t motors_number)
{
uint32_t *buffer = dshot_burst_buffer[timer];

16
src/drivers/drv_pwm_output.h

@ -408,6 +408,11 @@ __EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel); @@ -408,6 +408,11 @@ __EXPORT extern servo_position_t up_pwm_servo_get(unsigned channel);
*/
__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq);
/**
* Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method)
*/
__EXPORT extern void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry);
/**
* Set the current dshot throttle value for a channel (motor).
*
@ -415,7 +420,10 @@ __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq @@ -415,7 +420,10 @@ __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq
* @param throttle The output dshot throttle value in [0 = DSHOT_DISARM_VALUE, 1 = DSHOT_MIN_THROTTLE, 1999 = DSHOT_MAX_THROTTLE].
* @param telemetry If true, request telemetry from that motor
*/
__EXPORT extern void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry);
static inline void up_dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry)
{
dshot_motor_data_set(channel, throttle + DShot_cmd_MIN_throttle, telemetry);
}
/**
* Send DShot command to a channel (motor).
@ -424,7 +432,11 @@ __EXPORT extern void up_dshot_motor_data_set(unsigned channel, uint16_t throttle @@ -424,7 +432,11 @@ __EXPORT extern void up_dshot_motor_data_set(unsigned channel, uint16_t throttle
* @param command dshot_command_t
* @param telemetry If true, request telemetry from that motor
*/
__EXPORT extern void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry);
static inline void up_dshot_motor_command(unsigned channel, uint16_t command, bool telemetry)
{
dshot_motor_data_set(channel, command, telemetry);
}
/**
* Trigger dshot data transfer.

Loading…
Cancel
Save