Merge branch 'feature-oneshot125a' of https://github.com/nebbian/cleanflight into nebbian-feature-oneshot125a
This commit is contained in:
commit
d7e26980a8
|
@ -0,0 +1,61 @@
|
||||||
|
# Oneshot
|
||||||
|
|
||||||
|
Oneshot allows faster communication between the Flight Controller and the ESCs that are present on your multirotor.
|
||||||
|
|
||||||
|
It does this in two ways:
|
||||||
|
|
||||||
|
1. Use a signal that varies between 125 µs and 250 µs (instead of the normal PWM timing of 1000µs to 2000µs)
|
||||||
|
|
||||||
|
1. Only send a 'shot' once per flight controller loop, and do this as soon as the flight controller has calculated the required speed of the motors.
|
||||||
|
|
||||||
|
|
||||||
|
## Supported ESCs
|
||||||
|
|
||||||
|
At present, only the FlyDuino KISS ESCs are able to use the Oneshot125 protocol.
|
||||||
|
|
||||||
|
## Supported Boards
|
||||||
|
|
||||||
|
The Naze boards are supported, and have been flight tested in a number of configurations.
|
||||||
|
|
||||||
|
CC3D boards have been tested with a PPM receiver, however parallel PWM receivers might not work properly with this board.
|
||||||
|
|
||||||
|
## Enabling Oneshot
|
||||||
|
|
||||||
|
To configure Oneshot, you must turn off any power to your ESCs.
|
||||||
|
|
||||||
|
It is a good idea at this stage to configure your ESC for oneshot mode (by soldering JP1 in the case of the KISS ESC).
|
||||||
|
|
||||||
|
Connect a USB cable to your board, and connect using the Chrome GUI app.
|
||||||
|
|
||||||
|
Go to the CLI tab, and type the following:
|
||||||
|
|
||||||
|
|
||||||
|
feature ONESHOT125
|
||||||
|
save
|
||||||
|
|
||||||
|
|
||||||
|
Then you can safely power up your ESCs again.
|
||||||
|
|
||||||
|
|
||||||
|
## Configuration
|
||||||
|
|
||||||
|
The process for calibrating oneshot ESCs is the same as any other ESC.
|
||||||
|
|
||||||
|
1. Ensure that your ESCs are not powered up.
|
||||||
|
|
||||||
|
1. Connect to the board using a USB cable, and change to the motor test page.
|
||||||
|
|
||||||
|
1. Set the motor speed to maximum using the main slider.
|
||||||
|
|
||||||
|
1. Connect power to your ESCs. They will beep.
|
||||||
|
|
||||||
|
1. Click on the slider to bring the motor speed down to zero. The ESCs will beep again, usually a couple of times.
|
||||||
|
|
||||||
|
1. Disconnect the power from your ESCs.
|
||||||
|
|
||||||
|
1. Re-connect power to your ESCs, and verify that moving the motor slider makes your motors spin up normally.
|
||||||
|
|
||||||
|
|
||||||
|
## References
|
||||||
|
|
||||||
|
* FlyDuino (<a href="http://flyduino.net/">http://flyduino.net/</a>)
|
|
@ -39,7 +39,8 @@ typedef enum {
|
||||||
FEATURE_RX_MSP = 1 << 14,
|
FEATURE_RX_MSP = 1 << 14,
|
||||||
FEATURE_RSSI_ADC = 1 << 15,
|
FEATURE_RSSI_ADC = 1 << 15,
|
||||||
FEATURE_LED_STRIP = 1 << 16,
|
FEATURE_LED_STRIP = 1 << 16,
|
||||||
FEATURE_DISPLAY = 1 << 17
|
FEATURE_DISPLAY = 1 << 17,
|
||||||
|
FEATURE_ONESHOT125 = 1 << 18
|
||||||
} features_e;
|
} features_e;
|
||||||
|
|
||||||
bool feature(uint32_t mask);
|
bool feature(uint32_t mask);
|
||||||
|
|
|
@ -34,6 +34,7 @@
|
||||||
#define MAX_INPUTS 8
|
#define MAX_INPUTS 8
|
||||||
|
|
||||||
#define PWM_TIMER_MHZ 1
|
#define PWM_TIMER_MHZ 1
|
||||||
|
#define ONESHOT125_TIMER_MHZ 8
|
||||||
|
|
||||||
typedef struct drv_pwm_config_t {
|
typedef struct drv_pwm_config_t {
|
||||||
bool useParallelPWM;
|
bool useParallelPWM;
|
||||||
|
|
|
@ -27,6 +27,8 @@
|
||||||
|
|
||||||
#include "flight/failsafe.h" // FIXME dependency into the main code from a driver
|
#include "flight/failsafe.h" // FIXME dependency into the main code from a driver
|
||||||
|
|
||||||
|
#include "config/config.h" // FIXME dependency into the main code from a driver
|
||||||
|
|
||||||
#include "pwm_mapping.h"
|
#include "pwm_mapping.h"
|
||||||
|
|
||||||
#include "pwm_output.h"
|
#include "pwm_output.h"
|
||||||
|
@ -35,6 +37,7 @@ typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function poi
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
volatile timCCR_t *ccr;
|
volatile timCCR_t *ccr;
|
||||||
|
volatile TIM_TypeDef *tim;
|
||||||
uint16_t period;
|
uint16_t period;
|
||||||
pwmWriteFuncPtr pwmWritePtr;
|
pwmWriteFuncPtr pwmWritePtr;
|
||||||
} pwmOutputPort_t;
|
} pwmOutputPort_t;
|
||||||
|
@ -116,6 +119,7 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
p->period = period;
|
p->period = period;
|
||||||
|
p->tim = timerHardware->tim;
|
||||||
|
|
||||||
return p;
|
return p;
|
||||||
}
|
}
|
||||||
|
@ -136,6 +140,32 @@ void pwmWriteMotor(uint8_t index, uint16_t value)
|
||||||
motors[index]->pwmWritePtr(index, value);
|
motors[index]->pwmWritePtr(index, value);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void pwmFinishedWritingMotors(uint8_t numberMotors)
|
||||||
|
{
|
||||||
|
uint8_t index;
|
||||||
|
volatile TIM_TypeDef *lastTimerPtr = NULL;
|
||||||
|
|
||||||
|
|
||||||
|
if(feature(FEATURE_ONESHOT125)){
|
||||||
|
|
||||||
|
for(index = 0; index < numberMotors; index++){
|
||||||
|
|
||||||
|
// Force the timer to overflow if it's the first motor to output, or if we change timers
|
||||||
|
if(motors[index]->tim != lastTimerPtr){
|
||||||
|
lastTimerPtr = motors[index]->tim;
|
||||||
|
|
||||||
|
timerForceOverflow(motors[index]->tim);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again.
|
||||||
|
// This compare register will be set to the output value on the next main loop.
|
||||||
|
for(index = 0; index < numberMotors; index++){
|
||||||
|
*motors[index]->ccr = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void pwmWriteServo(uint8_t index, uint16_t value)
|
void pwmWriteServo(uint8_t index, uint16_t value)
|
||||||
{
|
{
|
||||||
if (servos[index] && index < MAX_SERVOS)
|
if (servos[index] && index < MAX_SERVOS)
|
||||||
|
@ -148,13 +178,18 @@ void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIn
|
||||||
uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
|
uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000;
|
||||||
motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, idlePulse);
|
motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, idlePulse);
|
||||||
motors[motorIndex]->pwmWritePtr = pwmWriteBrushed;
|
motors[motorIndex]->pwmWritePtr = pwmWriteBrushed;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
|
void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse)
|
||||||
{
|
{
|
||||||
uint32_t hz = PWM_TIMER_MHZ * 1000000;
|
uint32_t hz = PWM_TIMER_MHZ * 1000000;
|
||||||
|
|
||||||
|
if(feature(FEATURE_ONESHOT125)){
|
||||||
|
motors[motorIndex] = pwmOutConfig(timerHardware, ONESHOT125_TIMER_MHZ, 0xFFFF, idlePulse);
|
||||||
|
} else {
|
||||||
motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse);
|
motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse);
|
||||||
|
}
|
||||||
|
|
||||||
motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
|
motors[motorIndex]->pwmWritePtr = pwmWriteStandard;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -20,6 +20,7 @@
|
||||||
void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
|
void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
|
||||||
void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
|
void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse);
|
||||||
void pwmWriteMotor(uint8_t index, uint16_t value);
|
void pwmWriteMotor(uint8_t index, uint16_t value);
|
||||||
|
void pwmFinishedWritingMotors(uint8_t numberMotors);
|
||||||
|
|
||||||
void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
|
void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse);
|
||||||
void pwmWriteServo(uint8_t index, uint16_t value);
|
void pwmWriteServo(uint8_t index, uint16_t value);
|
||||||
|
|
|
@ -24,6 +24,8 @@
|
||||||
#include "build_config.h"
|
#include "build_config.h"
|
||||||
|
|
||||||
#include "common/utils.h"
|
#include "common/utils.h"
|
||||||
|
#include "config/config.h"
|
||||||
|
|
||||||
#include "system.h"
|
#include "system.h"
|
||||||
|
|
||||||
#include "nvic.h"
|
#include "nvic.h"
|
||||||
|
@ -80,6 +82,7 @@ static uint16_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT];
|
||||||
|
|
||||||
static uint8_t ppmFrameCount = 0;
|
static uint8_t ppmFrameCount = 0;
|
||||||
static uint8_t lastPPMFrameCount = 0;
|
static uint8_t lastPPMFrameCount = 0;
|
||||||
|
static uint8_t ppmCountShift = 0;
|
||||||
|
|
||||||
typedef struct ppmDevice {
|
typedef struct ppmDevice {
|
||||||
uint8_t pulseIndex;
|
uint8_t pulseIndex;
|
||||||
|
@ -157,6 +160,9 @@ static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture
|
||||||
/* Convert to 32-bit timer result */
|
/* Convert to 32-bit timer result */
|
||||||
ppmDev.currentTime += ppmDev.largeCounter;
|
ppmDev.currentTime += ppmDev.largeCounter;
|
||||||
|
|
||||||
|
// Divide by 8 if Oneshot125 is active and this is a CC3D board
|
||||||
|
ppmDev.currentTime = ppmDev.currentTime >> ppmCountShift;
|
||||||
|
|
||||||
/* Capture computation */
|
/* Capture computation */
|
||||||
ppmDev.deltaTime = ppmDev.currentTime - ppmDev.previousTime;
|
ppmDev.deltaTime = ppmDev.currentTime - ppmDev.previousTime;
|
||||||
|
|
||||||
|
@ -325,6 +331,11 @@ void ppmInConfig(const timerHardware_t *timerHardwarePtr)
|
||||||
|
|
||||||
timerConfigure(timerHardwarePtr, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ);
|
timerConfigure(timerHardwarePtr, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ);
|
||||||
|
|
||||||
|
if((timerHardwarePtr->tim == TIM4) && (feature(FEATURE_ONESHOT125))){
|
||||||
|
ppmCountShift = 3; // Divide by 8 if the timer is running at 8 MHz
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
timerChCCHandlerInit(&self->edgeCb, ppmEdgeCallback);
|
timerChCCHandlerInit(&self->edgeCb, ppmEdgeCallback);
|
||||||
timerChOvrHandlerInit(&self->overflowCb, ppmOverflowCallback);
|
timerChOvrHandlerInit(&self->overflowCb, ppmOverflowCallback);
|
||||||
timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb);
|
timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb);
|
||||||
|
|
|
@ -184,6 +184,7 @@ typedef struct timerConfig_s {
|
||||||
timerCCHandlerRec_t *edgeCallback[CC_CHANNELS_PER_TIMER];
|
timerCCHandlerRec_t *edgeCallback[CC_CHANNELS_PER_TIMER];
|
||||||
timerOvrHandlerRec_t *overflowCallback[CC_CHANNELS_PER_TIMER];
|
timerOvrHandlerRec_t *overflowCallback[CC_CHANNELS_PER_TIMER];
|
||||||
timerOvrHandlerRec_t *overflowCallbackActive; // null-terminated linkded list of active overflow callbacks
|
timerOvrHandlerRec_t *overflowCallbackActive; // null-terminated linkded list of active overflow callbacks
|
||||||
|
uint32_t forcedOverflowTimerValue;
|
||||||
} timerConfig_t;
|
} timerConfig_t;
|
||||||
timerConfig_t timerConfig[USED_TIMER_COUNT];
|
timerConfig_t timerConfig[USED_TIMER_COUNT];
|
||||||
|
|
||||||
|
@ -580,7 +581,14 @@ static void timCCxHandler(TIM_TypeDef *tim, timerConfig_t *timerConfig)
|
||||||
tim_status &= mask;
|
tim_status &= mask;
|
||||||
switch(bit) {
|
switch(bit) {
|
||||||
case __builtin_clz(TIM_IT_Update):
|
case __builtin_clz(TIM_IT_Update):
|
||||||
|
|
||||||
|
if(timerConfig->forcedOverflowTimerValue != 0){
|
||||||
|
capture = timerConfig->forcedOverflowTimerValue - 1;
|
||||||
|
timerConfig->forcedOverflowTimerValue = 0;
|
||||||
|
} else {
|
||||||
capture = tim->ARR;
|
capture = tim->ARR;
|
||||||
|
}
|
||||||
|
|
||||||
timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
|
timerOvrHandlerRec_t *cb = timerConfig->overflowCallbackActive;
|
||||||
while(cb) {
|
while(cb) {
|
||||||
cb->fn(cb, capture);
|
cb->fn(cb, capture);
|
||||||
|
@ -757,3 +765,22 @@ void timerStart(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Force an overflow for a given timer.
|
||||||
|
* Saves the current value of the counter in the relevant timerConfig's forcedOverflowTimerValue variable.
|
||||||
|
* @param TIM_Typedef *tim The timer to overflow
|
||||||
|
* @return void
|
||||||
|
**/
|
||||||
|
void timerForceOverflow(volatile TIM_TypeDef *tim)
|
||||||
|
{
|
||||||
|
uint8_t timerIndex = lookupTimerIndex((const TIM_TypeDef *)tim);
|
||||||
|
|
||||||
|
ATOMIC_BLOCK(NVIC_PRIO_TIMER) {
|
||||||
|
// Save the current count so that PPM reading will work on the same timer that was forced to overflow
|
||||||
|
timerConfig[timerIndex].forcedOverflowTimerValue = tim->CNT + 1;
|
||||||
|
|
||||||
|
// Force an overflow by setting the UG bit
|
||||||
|
tim->EGR |= TIM_EGR_UG;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -117,5 +117,7 @@ void timerChInit(const timerHardware_t *timHw, channelType_t type, int irqPriori
|
||||||
|
|
||||||
void timerInit(void);
|
void timerInit(void);
|
||||||
void timerStart(void);
|
void timerStart(void);
|
||||||
|
void timerForceOverflow(volatile TIM_TypeDef *tim);
|
||||||
|
|
||||||
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration
|
void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz); // TODO - just for migration
|
||||||
|
|
||||||
|
|
|
@ -395,6 +395,8 @@ void writeMotors(void)
|
||||||
|
|
||||||
for (i = 0; i < numberMotor; i++)
|
for (i = 0; i < numberMotor; i++)
|
||||||
pwmWriteMotor(i, motor[i]);
|
pwmWriteMotor(i, motor[i]);
|
||||||
|
|
||||||
|
pwmFinishedWritingMotors(numberMotor);
|
||||||
}
|
}
|
||||||
|
|
||||||
void writeAllMotors(int16_t mc)
|
void writeAllMotors(int16_t mc)
|
||||||
|
|
|
@ -123,7 +123,7 @@ static const char * const featureNames[] = {
|
||||||
"RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP",
|
"RX_PPM", "VBAT", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP",
|
||||||
"SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE",
|
"SERVO_TILT", "SOFTSERIAL", "GPS", "FAILSAFE",
|
||||||
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
|
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
|
||||||
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", NULL
|
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "ONESHOT125", NULL
|
||||||
};
|
};
|
||||||
|
|
||||||
// sync this with sensors_e
|
// sync this with sensors_e
|
||||||
|
|
Loading…
Reference in New Issue