mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-07 17:49:49 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -76,18 +76,13 @@ static inline void main_periodic_task( void ) {
|
||||
float rpm = mb_tacho_get_averaged();
|
||||
mb_current_periodic();
|
||||
float amps = mb_current_amp;
|
||||
mb_scale_periodic();
|
||||
float thrust = mb_scale_thrust;
|
||||
float torque = 0.;
|
||||
|
||||
static uint8_t my_cnt = 0;
|
||||
my_cnt++;
|
||||
if ( my_cnt >= 125 ) {
|
||||
// LED_TOGGLE(1);
|
||||
my_cnt = 0;
|
||||
DOWNLINK_SEND_ALIVE(16, MD5SUM);
|
||||
PeriodicSendDlValue();
|
||||
}
|
||||
RunOnceEvery(125, {
|
||||
DOWNLINK_SEND_ALIVE(16, MD5SUM);
|
||||
PeriodicSendDlValue();
|
||||
});
|
||||
DOWNLINK_SEND_MOTOR_BENCH_STATUS(&cpu_time_ticks, &throttle, &rpm, &s , &thrust, &torque, &cpu_time_sec, &mb_modes_mode);
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
#include "mb_buss_twi_controller.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "i2c.h"
|
||||
|
||||
uint8_t mb_buss_twi_command;
|
||||
|
||||
uint8_t mb_buss_twi_nb_overun;
|
||||
uint8_t mb_buss_twi_i2c_done;
|
||||
|
||||
|
||||
#define MB_BUSS_TWI_CONTROLLER_MAX_CMD 255
|
||||
/*
|
||||
Slave address
|
||||
front = 0x52
|
||||
back = 0x54
|
||||
right = 0x56
|
||||
left = 0x58
|
||||
*/
|
||||
#define MB_BUSS_TWI_CONTROLLER_ADDR 0x52
|
||||
|
||||
void mb_buss_twi_controller_init(void) {
|
||||
mb_buss_twi_nb_overun = 0;
|
||||
mb_buss_twi_i2c_done = TRUE;
|
||||
}
|
||||
|
||||
void mb_buss_twi_controller_set( float throttle ) {
|
||||
if (mb_buss_twi_i2c_done) {
|
||||
mb_buss_twi_command = throttle * MB_BUSS_TWI_CONTROLLER_MAX_CMD;
|
||||
i2c_buf[0] = mb_buss_twi_command;
|
||||
i2c_transmit(MB_BUSS_TWI_CONTROLLER_ADDR, 1, &mb_buss_twi_i2c_done);
|
||||
}
|
||||
else
|
||||
mb_buss_twi_nb_overun++;
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
#ifndef MB_BUSS_TWI_CONTROLLER_H
|
||||
#define MB_BUSS_TWI_CONTROLLER_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
extern void mb_buss_twi_controller_init(void);
|
||||
|
||||
extern void mb_buss_twi_controller_set( float throttle );
|
||||
|
||||
#endif /* MB_BUSS_TWI_CONTROLLER_H */
|
||||
|
||||
|
||||
@@ -0,0 +1,17 @@
|
||||
#include "mb_current.h"
|
||||
|
||||
#include "adc.h"
|
||||
|
||||
static struct adc_buf mb_current_buf;
|
||||
|
||||
float mb_current_amp;
|
||||
|
||||
void mb_current_init(void) {
|
||||
adc_buf_channel(4, &mb_current_buf, 16);
|
||||
|
||||
}
|
||||
|
||||
|
||||
void mb_current_periodic(void) {
|
||||
mb_current_amp = (float)mb_current_buf.sum * 0.00113607 - 2.8202;
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
#ifndef MB_CURRENT_H
|
||||
#define MB_CURRENT_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
extern void mb_current_init(void);
|
||||
extern void mb_current_periodic(void);
|
||||
|
||||
extern float mb_current_amp;
|
||||
|
||||
#endif /* MB_CURRENT_H */
|
||||
@@ -0,0 +1,93 @@
|
||||
#include "mb_modes.h"
|
||||
|
||||
#include "adc.h"
|
||||
#include "sys_time.h"
|
||||
|
||||
|
||||
uint8_t mb_modes_mode;
|
||||
float mb_modes_throttle;
|
||||
|
||||
float mb_modes_last_change_time;
|
||||
|
||||
float mb_modes_ramp_duration;
|
||||
|
||||
float mb_modes_step_low_throttle;
|
||||
float mb_modes_step_high_throttle;
|
||||
float mb_modes_step_duration;
|
||||
|
||||
static void mb_modes_manual( void );
|
||||
static void mb_modes_ramp( void );
|
||||
static void mb_modes_step( void );
|
||||
static void mb_modes_prbs( void );
|
||||
|
||||
|
||||
static struct adc_buf mb_modes_adc_buf; /* manual mode */
|
||||
|
||||
void mb_mode_init(void) {
|
||||
adc_buf_channel(1, &mb_modes_adc_buf, 16);
|
||||
mb_modes_mode = MB_MODES_IDLE;
|
||||
mb_modes_throttle = 0.;
|
||||
|
||||
mb_modes_ramp_duration = 200;
|
||||
|
||||
mb_modes_step_low_throttle = 0.6;
|
||||
mb_modes_step_high_throttle = 0.7;
|
||||
mb_modes_step_duration = 1.;
|
||||
|
||||
}
|
||||
|
||||
void mb_mode_event(void) {}
|
||||
|
||||
void mb_mode_periodic(void) {
|
||||
switch (mb_modes_mode) {
|
||||
case MB_MODES_IDLE :
|
||||
mb_modes_throttle = 0.;
|
||||
break;
|
||||
case MB_MODES_MANUAL :
|
||||
mb_modes_manual();
|
||||
break;
|
||||
case MB_MODES_RAMP :
|
||||
mb_modes_ramp();
|
||||
break;
|
||||
case MB_MODES_STEP :
|
||||
mb_modes_step();
|
||||
break;
|
||||
default:
|
||||
mb_modes_throttle = 0.;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void mb_modes_manual( void ) {
|
||||
uint16_t poti = mb_modes_adc_buf.sum;
|
||||
mb_modes_throttle = (float)poti/(16.*1024.);
|
||||
}
|
||||
|
||||
static void mb_modes_ramp( void ) {
|
||||
float now = GET_CUR_TIME_FLOAT();
|
||||
float elapsed = now - mb_modes_last_change_time;
|
||||
if ( elapsed < mb_modes_ramp_duration)
|
||||
mb_modes_throttle = elapsed/mb_modes_ramp_duration;
|
||||
else if ( elapsed < 2 * mb_modes_ramp_duration)
|
||||
mb_modes_throttle = 2 - elapsed/mb_modes_ramp_duration;
|
||||
else {
|
||||
mb_modes_last_change_time = now;
|
||||
mb_modes_throttle = 0.;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void mb_modes_step( void ) {
|
||||
float now = GET_CUR_TIME_FLOAT();
|
||||
float elapsed = now - mb_modes_last_change_time;
|
||||
if ( elapsed < mb_modes_step_duration)
|
||||
mb_modes_throttle = mb_modes_step_low_throttle;
|
||||
else if ( elapsed < 2 * mb_modes_step_duration)
|
||||
mb_modes_throttle = mb_modes_step_high_throttle;
|
||||
else {
|
||||
mb_modes_last_change_time = now;
|
||||
mb_modes_throttle = mb_modes_step_low_throttle;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,34 @@
|
||||
#ifndef MB_MODES_H
|
||||
#define MB_MODES_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
|
||||
#define MB_MODES_IDLE 0
|
||||
#define MB_MODES_MANUAL 1
|
||||
#define MB_MODES_RAMP 2
|
||||
#define MB_MODES_STEP 3
|
||||
#define MB_MODES_PRBS 4
|
||||
|
||||
extern uint8_t mb_modes_mode;
|
||||
extern float mb_modes_throttle;
|
||||
|
||||
extern float mb_modes_last_change_time;
|
||||
|
||||
extern float mb_modes_ramp_duration;
|
||||
|
||||
extern float mb_modes_step_low_throttle;
|
||||
extern float mb_modes_step_high_throttle;
|
||||
extern float mb_modes_step_duration;
|
||||
|
||||
|
||||
extern void mb_mode_init(void);
|
||||
extern void mb_mode_event(void);
|
||||
extern void mb_mode_periodic(void);
|
||||
|
||||
#define mb_modes_SetMode(_val) { \
|
||||
mb_modes_mode = _val; \
|
||||
mb_modes_last_change_time = GET_CUR_TIME_FLOAT(); \
|
||||
}
|
||||
|
||||
#endif /* MB_MODES_H */
|
||||
@@ -0,0 +1,15 @@
|
||||
#include "mb_scale.h"
|
||||
|
||||
volatile uint32_t mb_scale_pulse_len;
|
||||
float mb_scale_thrust;
|
||||
float mb_scale_torque;
|
||||
|
||||
uint32_t mb_scale_neutral = 2906400;
|
||||
float mb_scale_gain = 0.0021563;
|
||||
|
||||
void mb_scale_init ( void ) {
|
||||
/* select pin for capture */
|
||||
ICP_PINSEL |= ICP_PINSEL_VAL << ICP_PINSEL_BIT;
|
||||
/* enable capture 0.3 on falling edge + trigger interrupt */
|
||||
T0CCR |= TCCR_CR3_F | TCCR_CR3_I;
|
||||
}
|
||||
@@ -0,0 +1,33 @@
|
||||
#ifndef MB_SCALE_H
|
||||
#define MB_SCALE_H
|
||||
|
||||
#include "std.h"
|
||||
#include "LPC21xx.h"
|
||||
|
||||
/* P0.29 CAP0.3 */
|
||||
#define ICP_PINSEL PINSEL1
|
||||
#define ICP_PINSEL_VAL 0x02
|
||||
#define ICP_PINSEL_BIT 26
|
||||
|
||||
extern volatile uint32_t mb_scale_pulse_len;
|
||||
extern float mb_scale_thrust;
|
||||
extern float mb_scale_torque;
|
||||
|
||||
extern uint32_t mb_scale_neutral;
|
||||
extern float mb_scale_gain;
|
||||
|
||||
void mb_scale_init ( void );
|
||||
|
||||
|
||||
#define MB_SCALE_ICP_ISR() { \
|
||||
static uint32_t last; \
|
||||
uint32_t now = T0CR3; \
|
||||
mb_scale_pulse_len = now - last; \
|
||||
mb_scale_thrust = mb_scale_gain * \
|
||||
( mb_scale_pulse_len - mb_scale_neutral); \
|
||||
last = now; \
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* MB_SCALE_H */
|
||||
@@ -0,0 +1,66 @@
|
||||
#include "mb_servo.h"
|
||||
|
||||
#include "sys_time.h"
|
||||
#define MY_NB_CLOCK_TIMER_PWM(time_us) SYS_TICS_OF_USEC(time_us)
|
||||
|
||||
uint32_t mb_servo_max_pulse_ns, mb_servo_min_pulse_ns;
|
||||
|
||||
void mb_servo_set_ns(uint32_t duration_ns);
|
||||
|
||||
void mb_servo_init( void ) {
|
||||
/* set P0.21 as PWM5 output */
|
||||
PINSEL1 |= (0X01 << 10);
|
||||
/* enable and select the type of PWM channel */
|
||||
PWMPCR |= PWMPCR_ENA5;
|
||||
/* set Match0 value (refresh rate) */
|
||||
PWMMR0 = MY_NB_CLOCK_TIMER_PWM(25000);
|
||||
/* commit PWMMRx changes */
|
||||
PWMLER = PWMLER_LATCH0;
|
||||
/* enable PWM timer in PWM mode */
|
||||
PWMTCR = PWMTCR_COUNTER_ENABLE | PWMTCR_PWM_ENABLE;
|
||||
mb_servo_min_pulse_ns = MIN_SERVO_NS;
|
||||
mb_servo_max_pulse_ns = MAX_SERVO_NS;
|
||||
}
|
||||
|
||||
void mb_servo_set_range( uint32_t min_pulse_ns, uint32_t max_pulse_ns ) {
|
||||
mb_servo_min_pulse_ns = min_pulse_ns;
|
||||
mb_servo_max_pulse_ns = max_pulse_ns;
|
||||
}
|
||||
|
||||
void mb_servo_set_us(uint32_t duration_us) {
|
||||
/* set Match5 value (pulse duration )*/
|
||||
PWMMR5 = MY_NB_CLOCK_TIMER_PWM(duration_us);
|
||||
/* commit PWMMRx changes */
|
||||
PWMLER = PWMLER_LATCH5;
|
||||
}
|
||||
|
||||
void mb_servo_set_ns(uint32_t duration_ns) {
|
||||
/* set Match5 value (pulse duration )*/
|
||||
PWMMR5 = SYS_TICS_OF_NSEC(duration_ns);
|
||||
/* commit PWMMRx changes */
|
||||
PWMLER = PWMLER_LATCH5;
|
||||
}
|
||||
|
||||
/* normalized throttle between 0. and 1. */
|
||||
void mb_servo_set(float throttle) {
|
||||
uint32_t range = mb_servo_max_pulse_ns - mb_servo_min_pulse_ns;
|
||||
uint32_t pulse_ns = mb_servo_min_pulse_ns + throttle * (range);
|
||||
mb_servo_set_ns(pulse_ns);
|
||||
}
|
||||
|
||||
#define FOO_DELAY() { \
|
||||
uint32_t foo = 0; \
|
||||
while (foo<10000000) foo++; \
|
||||
}
|
||||
|
||||
|
||||
/* arm the brushless controller */
|
||||
|
||||
void mb_servo_arm (void) {
|
||||
mb_servo_set(0.);
|
||||
FOO_DELAY();
|
||||
mb_servo_set(1.);
|
||||
FOO_DELAY();
|
||||
mb_servo_set(0.);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
|
||||
#ifndef MB_SERVO_H
|
||||
#define MB_SERVO_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
#define MIN_SERVO_US 1000
|
||||
#define MAX_SERVO_US 2000
|
||||
#define MIN_SERVO_NS 1000000
|
||||
#define MAX_SERVO_NS 2000000
|
||||
#define RANGE_SERVO_US (MAX_SERVO_US - MIN_SERVO_US)
|
||||
|
||||
void mb_servo_init( void );
|
||||
void mb_servo_set_range( uint32_t min_pulse_ns, uint32_t max_pulse_ns );
|
||||
void mb_servo_set_us(uint32_t duration_us);
|
||||
//void mb_servo_set_ns(uint32_t duration_ns);
|
||||
void mb_servo_set(float throttle);
|
||||
void mb_servo_arm (void);
|
||||
#endif /* MB_SERVO_H */
|
||||
@@ -0,0 +1,60 @@
|
||||
#include "mb_tacho.h"
|
||||
|
||||
#include "LPC21xx.h"
|
||||
|
||||
#include "interrupt_hw.h"
|
||||
|
||||
volatile uint32_t mb_tacho_duration;
|
||||
volatile uint8_t got_one_pulse;
|
||||
volatile float mb_tacho_averaged;
|
||||
volatile uint16_t mb_tacho_nb_pulse;
|
||||
|
||||
/* INPUT CAPTURE CAP0.0 on P0.22*/
|
||||
#define MB_TACHO_PINSEL PINSEL1
|
||||
#define MB_TACHO_PINSEL_VAL 0x02
|
||||
#define MB_TACHO_PINSEL_BIT 12
|
||||
|
||||
#define MB_TACHO_NB_SLOT 65
|
||||
//#define MB_TACHO_NB_SLOT 36
|
||||
|
||||
void mb_tacho_init ( void ) {
|
||||
/* select pin for capture */
|
||||
MB_TACHO_PINSEL |= MB_TACHO_PINSEL_VAL << MB_TACHO_PINSEL_BIT;
|
||||
/* enable capture 0.2 on falling edge + trigger interrupt */
|
||||
T0CCR |= TCCR_CR0_F | TCCR_CR0_I;
|
||||
}
|
||||
|
||||
uint32_t mb_tacho_get_duration( void ) {
|
||||
int_disable();
|
||||
uint32_t my_duration = 0;
|
||||
if (got_one_pulse) {
|
||||
my_duration = mb_tacho_duration;
|
||||
}
|
||||
got_one_pulse = FALSE;
|
||||
int_enable();
|
||||
return my_duration;
|
||||
}
|
||||
|
||||
float mb_tacho_get_averaged( void ) {
|
||||
|
||||
int_disable();
|
||||
float ret;
|
||||
float tacho;
|
||||
const float tach_to_rpm = 15000000.*60./(float)MB_TACHO_NB_SLOT;
|
||||
if (mb_tacho_nb_pulse)
|
||||
tacho = mb_tacho_averaged / (float)mb_tacho_nb_pulse ;
|
||||
else
|
||||
tacho = 0.;
|
||||
|
||||
if (tacho ==0)
|
||||
ret = 0;
|
||||
else
|
||||
ret = tach_to_rpm/tacho;
|
||||
|
||||
mb_tacho_averaged = 0.;
|
||||
mb_tacho_nb_pulse = 0;
|
||||
int_enable();
|
||||
|
||||
return ret;
|
||||
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
#ifndef MB_TACHO_H
|
||||
#define MB_TACHO_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
extern void mb_tacho_init ( void );
|
||||
extern uint32_t mb_tacho_get_duration( void );
|
||||
extern float mb_tacho_get_averaged( void );
|
||||
|
||||
extern volatile uint32_t mb_tacho_duration;
|
||||
extern volatile uint8_t got_one_pulse;
|
||||
extern volatile float mb_tacho_averaged;
|
||||
extern volatile uint16_t mb_tacho_nb_pulse;
|
||||
|
||||
#define MB_TACHO_ISR() { \
|
||||
static uint32_t tmb_last; \
|
||||
uint32_t t_now = T0CR0; \
|
||||
uint32_t diff = t_now - tmb_last; \
|
||||
mb_tacho_duration = diff; \
|
||||
mb_tacho_averaged += diff; \
|
||||
mb_tacho_nb_pulse++; \
|
||||
tmb_last = t_now; \
|
||||
got_one_pulse = TRUE; \
|
||||
}
|
||||
|
||||
#endif /* MB_TACHO_H */
|
||||
@@ -0,0 +1,28 @@
|
||||
#include "mb_twi_controller.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "i2c.h"
|
||||
|
||||
uint16_t mb_twi_command;
|
||||
|
||||
uint8_t mb_twi_nb_overun;
|
||||
uint8_t mb_twi_i2c_done;
|
||||
|
||||
|
||||
void mb_twi_controller_init(void) {
|
||||
mb_twi_nb_overun = 0;
|
||||
mb_twi_i2c_done = TRUE;
|
||||
}
|
||||
|
||||
void mb_twi_controller_set( float throttle ) {
|
||||
if (mb_twi_i2c_done) {
|
||||
mb_twi_command = throttle * MB_TWI_CONTROLLER_MAX_CMD;
|
||||
i2c_buf[0] = (uint8_t)(mb_twi_command&0xFF);
|
||||
i2c_buf[1] = (uint8_t)(mb_twi_command>>8);
|
||||
i2c_transmit(MB_TWI_CONTROLLER_ADDR, 2, &mb_twi_i2c_done);
|
||||
}
|
||||
else
|
||||
mb_twi_nb_overun++;
|
||||
|
||||
}
|
||||
@@ -7,6 +7,16 @@ extern void mb_twi_controller_init(void);
|
||||
|
||||
extern void mb_twi_controller_set( float throttle );
|
||||
|
||||
#define MB_TWI_CONTROLLER_MAX_CMD 65535
|
||||
/*
|
||||
Slave address
|
||||
front = 0x52
|
||||
back = 0x54
|
||||
right = 0x56
|
||||
left = 0x58
|
||||
*/
|
||||
#define MB_TWI_CONTROLLER_ADDR 0x52
|
||||
|
||||
#endif /* MB_TWI_CONTROLLER_H */
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user