/** * here we configure all possible channels for slow mode. Some channels would not actually * be used hopefully that's fine to configure all possible channels. */ ADC_SMPR1_SMP_AN10(ADC_SAMPLING_SLOW) | ADC_SMPR1_SMP_AN11(ADC_SAMPLING_SLOW) | ADC_SMPR1_SMP_AN12(ADC_SAMPLING_SLOW) | ADC_SMPR1_SMP_AN13(ADC_SAMPLING_SLOW) | ADC_SMPR1_SMP_AN14(ADC_SAMPLING_SLOW) | ADC_SMPR1_SMP_AN15(ADC_SAMPLING_SLOW) | ADC_SMPR1_SMP_SENSOR(ADC_SAMPLE_144) , // sample times for channels 10...18 ADC_SMPR2_SMP_AN0(ADC_SAMPLING_SLOW) | ADC_SMPR2_SMP_AN1(ADC_SAMPLING_SLOW) | ADC_SMPR2_SMP_AN2(ADC_SAMPLING_SLOW) | ADC_SMPR2_SMP_AN3(ADC_SAMPLING_SLOW) | ADC_SMPR2_SMP_AN4(ADC_SAMPLING_SLOW) | ADC_SMPR2_SMP_AN5(ADC_SAMPLING_SLOW) | ADC_SMPR2_SMP_AN6(ADC_SAMPLING_SLOW) | ADC_SMPR2_SMP_AN7(ADC_SAMPLING_SLOW) | ADC_SMPR2_SMP_AN8(ADC_SAMPLING_SLOW) | ADC_SMPR2_SMP_AN9(ADC_SAMPLING_SLOW) , // In this field must be specified the sample times for channels 0...9 0, // Conversion group sequence 13...16 + sequence length 0, // Conversion group sequence 7...12 0 // Conversion group sequence 1...6 }; AdcDevice slowAdc(&adcgrpcfgSlow);
*/
static ADCConversionGroup adcgrpcfgSlow = { FALSE, 0, adc_callback_slow, NULL,
/* HW dependent part.*/
ADC_TwoSamplingDelay_20Cycles, // cr1
ADC_CR2_SWSTART, // cr2
ADC_SMPR1_SMP_AN10(MY_SAMPLING_SLOW) |
ADC_SMPR1_SMP_AN11(MY_SAMPLING_SLOW) |
ADC_SMPR1_SMP_AN12(MY_SAMPLING_SLOW) |
ADC_SMPR1_SMP_AN13(MY_SAMPLING_SLOW) |
ADC_SMPR1_SMP_AN14(MY_SAMPLING_SLOW) |
ADC_SMPR1_SMP_AN15(MY_SAMPLING_SLOW)
, // sample times for channels 10...18
ADC_SMPR2_SMP_AN0(MY_SAMPLING_SLOW) |
ADC_SMPR2_SMP_AN1(MY_SAMPLING_SLOW) |
ADC_SMPR2_SMP_AN3(MY_SAMPLING_SLOW) |
ADC_SMPR2_SMP_AN4(MY_SAMPLING_SLOW) |
ADC_SMPR2_SMP_AN5(MY_SAMPLING_SLOW) |
ADC_SMPR2_SMP_AN6(MY_SAMPLING_SLOW) |
ADC_SMPR2_SMP_AN7(MY_SAMPLING_SLOW) |
ADC_SMPR2_SMP_AN8(MY_SAMPLING_SLOW) |
ADC_SMPR2_SMP_AN9(MY_SAMPLING_SLOW)
, // In this field must be specified the sample times for channels 0...9
0, // Conversion group sequence 13...16 + sequence length
0, // Conversion group sequence 7...12
0 // Conversion group sequence 1...6
};
AdcDevice slowAdc(&adcgrpcfgSlow);
/*
* ADC conversion group.
* Mode: Linear buffer, 8 samples of 1 channel, SW triggered.
* Channels: IN11.
*/
static const ADCConversionGroup adcgrpcfg1 = {FALSE, //circular buffer mode
ADC_GRP1_NUM_CHANNELS, //Number of the analog channels
NULL, //Callback function (not needed here)
0, //Error callback
0, /* CR1 */
ADC_CR2_SWSTART, /* CR2 */
ADC_SMPR1_SMP_AN10(ADC_SAMPLE_84) | ADC_SMPR1_SMP_AN11(ADC_SAMPLE_84)
| ADC_SMPR1_SMP_AN12(ADC_SAMPLE_84) | ADC_SMPR1_SMP_AN13(ADC_SAMPLE_84)
| ADC_SMPR1_SMP_AN14(ADC_SAMPLE_84) | ADC_SMPR1_SMP_AN15(ADC_SAMPLE_84), //sample times ch10-18
ADC_SMPR2_SMP_AN0(ADC_SAMPLE_84) | ADC_SMPR2_SMP_AN1(ADC_SAMPLE_84)
| ADC_SMPR2_SMP_AN2(ADC_SAMPLE_84) | ADC_SMPR2_SMP_AN3(ADC_SAMPLE_84)
| ADC_SMPR2_SMP_AN4(ADC_SAMPLE_84) | ADC_SMPR2_SMP_AN5(ADC_SAMPLE_84)
| ADC_SMPR2_SMP_AN6(ADC_SAMPLE_84) | ADC_SMPR2_SMP_AN7(ADC_SAMPLE_84)
| ADC_SMPR2_SMP_AN8(ADC_SAMPLE_84) | ADC_SMPR2_SMP_AN9(ADC_SAMPLE_84), //sample times ch0-9
ADC_SQR1_SQ13_N(ADC_CHANNEL_IN12) | ADC_SQR1_SQ14_N(ADC_CHANNEL_IN13)
| ADC_SQR1_SQ15_N(ADC_CHANNEL_IN14) | ADC_SQR1_SQ16_N(ADC_CHANNEL_IN15)
| ADC_SQR1_NUM_CH(ADC_GRP1_NUM_CHANNELS), //SQR1: Conversion group sequence 13...16 + sequence length
ADC_SQR2_SQ7_N(ADC_CHANNEL_IN6) | ADC_SQR2_SQ8_N(ADC_CHANNEL_IN7)
| ADC_SQR2_SQ9_N(ADC_CHANNEL_IN8) | ADC_SQR2_SQ10_N(ADC_CHANNEL_IN9)
| ADC_SQR2_SQ11_N(ADC_CHANNEL_IN10) | ADC_SQR2_SQ12_N(ADC_CHANNEL_IN11), //SQR2: Conversion group sequence 7...12
ADC_SQR3_SQ1_N(ADC_CHANNEL_IN0) | ADC_SQR3_SQ2_N(ADC_CHANNEL_IN1)
| ADC_SQR3_SQ3_N(ADC_CHANNEL_IN2) | ADC_SQR3_SQ4_N(ADC_CHANNEL_IN3)
| ADC_SQR3_SQ5_N(ADC_CHANNEL_IN4) | ADC_SQR3_SQ6_N(ADC_CHANNEL_IN5) //SQR3: Conversion group sequence 1...6
};
void adc_convert(void) {
// ADC_SAMPLE_13P5
// ADC_SAMPLE_28P5
// ADC_SAMPLE_41P5
// ADC_SAMPLE_55P5
// ADC_SAMPLE_71P5
// ADC_SAMPLE_239P5
static const ADCConversionGroup adcGroup =
{
TRUE,
ADC_NUM_CHANNELS,
contAdcReadyCb,
NULL,
0, 0, /* CR1, CR2 */
0,
ADC_SMPR2_SMP_AN2(ADC_SAMPLE_13P5) | ADC_SMPR2_SMP_AN3( ADC_SAMPLE_13P5 ) |
ADC_SMPR2_SMP_AN4( ADC_SAMPLE_13P5 ),
ADC_SQR1_NUM_CH( ADC_NUM_CHANNELS ),
0,
ADC_SQR3_SQ1_N(ADC_CHANNEL_IN2) |
ADC_SQR3_SQ2_N(ADC_CHANNEL_IN3) |
ADC_SQR3_SQ3_N(ADC_CHANNEL_IN4)
};
static PWMConfig pwmCfg =
{
48000000, // 1000kHz PWM clock frequency.
480, // Initial PWM period 10us.
NULL,
{
{ PWM_OUTPUT_ACTIVE_HIGH, NULL },
namespace r2p {
/*===========================================================================*/
/* Motor calibration. */
/*===========================================================================*/
static int pwm = 0;
#define ADC_NUM_CHANNELS 1
#define ADC_BUF_DEPTH 1
static float meanLevel = 0.0f;
static adcsample_t adc_samples[ADC_NUM_CHANNELS * ADC_BUF_DEPTH];
static void current_callback(ADCDriver *adcp, adcsample_t *buffer, size_t n);
/*
* ADC conversion group.
* Mode: Circular buffer, 1 sample of 1 channel, triggered by pwm channel 3
* Channels: IN10.
*/
static const ADCConversionGroup adcgrpcfg = { TRUE, // circular
ADC_NUM_CHANNELS, // num channels
current_callback, // end callback
NULL, // error callback
0, // CR1
ADC_CR2_EXTTRIG | ADC_CR2_EXTSEL_1, // CR2
0, // SMPR1
ADC_SMPR2_SMP_AN3(ADC_SAMPLE_1P5)/*ADC_SMPR2_SMP_AN3(ADC_SAMPLE_239P5)*/, // SMPR2
ADC_SQR1_NUM_CH(ADC_NUM_CHANNELS), // SQR1
0, // SQR2
ADC_SQR3_SQ1_N(ADC_CHANNEL_IN3) // SQR3
};
static void current_callback(ADCDriver *adcp, adcsample_t *buffer, size_t n) {
(void) adcp;
(void) n;
palTogglePad(LED2_GPIO, LED2);
chSysLockFromIsr()
;
meanLevel = buffer[0];
if (tp_motor != NULL) {
chSchReadyI(tp_motor);
tp_motor = NULL;
}
chSysUnlockFromIsr();
}
static PWMConfig pwmcfg = { STM32_SYSCLK, // 72MHz PWM clock frequency.
4096, // 12-bit PWM, 17KHz frequency.
NULL, // pwm callback
{
{ PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_HIGH, NULL }, //
{ PWM_OUTPUT_ACTIVE_HIGH | PWM_COMPLEMENTARY_OUTPUT_ACTIVE_HIGH, NULL }, //
{ PWM_OUTPUT_ACTIVE_LOW, NULL }, //
{ PWM_OUTPUT_DISABLED, NULL } }, //
0, //
#if STM32_PWM_USE_ADVANCED
72, /* XXX 1uS deadtime insertion */
#endif
0 };
static calibration_pub_node_conf defaultPubConf = { "motor_calibration_node",
"bits" };
msg_t motor_calibration_node(void * arg) {
//Configure current node
calibration_pub_node_conf* conf;
if (arg != NULL)
conf = (calibration_pub_node_conf *) arg;
else
conf = &defaultPubConf;
Node node(conf->name);
Publisher<FloatMsg> current_pub;
FloatMsg * msgp;
chRegSetThreadName(conf->name);
node.advertise(current_pub, conf->topic);
// Start the ADC driver and conversion
adcStart(&ADC_DRIVER, NULL);
adcStartConversion(&ADC_DRIVER, &adcgrpcfg, adc_samples, ADC_BUF_DEPTH);
// Init motor driver
palSetPad(DRIVER_GPIO, DRIVER_RESET);
chThdSleepMilliseconds(500);
pwmStart(&PWM_DRIVER, &pwmcfg);
// wait some time
chThdSleepMilliseconds(500);
// start pwm
float voltage = 24.0;
const float pwm_res = 4096.0f / 24.0f;
pwm = static_cast<int>(voltage * pwm_res);
if (pwm > 0) {
pwm_lld_enable_channel(&PWM_DRIVER, 1, pwm);
pwm_lld_enable_channel(&PWM_DRIVER, 0, 0);
pwm_lld_enable_channel(&PWM_DRIVER, 2, pwm/2);
} else {
pwm_lld_enable_channel(&PWM_DRIVER, 1, 0);
pwm_lld_enable_channel(&PWM_DRIVER, 0, -pwm);
pwm_lld_enable_channel(&PWM_DRIVER, 2, -pwm/2);
}
// Start publishing current measures
for (;;) {
// Wait for interrupt
chSysLock()
;
tp_motor = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
chSysUnlock();
// publish current
if (current_pub.alloc(msgp)) {
msgp->value = meanLevel;
current_pub.publish(*msgp);
}
}
return CH_SUCCESS;
}
}
