Browse Source

working rc->mixer->blctrls path

master
Olaf Rempel 12 years ago
parent
commit
a8256718ab
5 changed files with 121 additions and 66 deletions
  1. +1
    -0
      include/at91_adc.h
  2. +2
    -0
      include/at91_tc1.h
  3. +66
    -31
      main.c
  4. +29
    -35
      src/at91_adc.c
  5. +23
    -0
      src/at91_tc1.c

+ 1
- 0
include/at91_adc.h View File

@@ -11,6 +11,7 @@
#define ADC_ACC_YAW 5
#define ADC_VOLTAGE 6

#define ADC_CAL_NONE 0
#define ADC_CAL_GYRO 1
#define ADC_CAL_ACC 2



+ 2
- 0
include/at91_tc1.h View File

@@ -11,6 +11,8 @@ struct rc_values {
};

uint32_t rcontrol_getvalues(struct rc_values *rc);
uint32_t rcontrol_getswitches(struct rc_values *rc);

void rcontrol_calibrate(uint32_t mode);
void rcontrol_print_cal(void);



+ 66
- 31
main.c View File

@@ -33,54 +33,90 @@
#include <stdio.h>
#include <string.h>
static uint32_t pitc_test(struct pitc_timer *timer)
static uint32_t global_state;
#define MOTORS_RUNNING 0x0001
#define STICK_CALIBRATION 0x0002
static uint32_t stick_timer_cb(struct pitc_timer *timer)
{
static uint32_t i;
*AT91C_PIOA_SODR = i;
i = i ^ LED_GREEN;
*AT91C_PIOA_CODR = i;
/*
struct rc_values rc;
uint32_t count = rcontrol_getvalues(&rc);
uint32_t count = rcontrol_getswitches(&rc);
printf("%ld channels: ", count);
uint32_t j;
for (j = 0; j < count; j++)
printf("%+5d ", rc.chan[j]);
static uint32_t stick_cal_count;
printf("\r");
if (global_state & MOTORS_RUNNING) {
if ((rc.chan[2] < 0 && rc.chan[3] > 0) || count < 4)
global_state &= ~MOTORS_RUNNING;
static uint32_t cnt;
} else if (global_state & STICK_CALIBRATION) {
stick_cal_count--;
if (stick_cal_count == 0) {
rcontrol_calibrate(RC_CAL_END);
rcontrol_print_cal();
global_state &= ~STICK_CALIBRATION;
}
if (cnt++ == 300) {
rcontrol_calibrate(RC_CAL_END);
printf("\n\r");
rcontrol_print_cal();
}
*/
} else {
if (rc.chan[2] < 0 && rc.chan[3] < 0)
global_state |= MOTORS_RUNNING;
adc_trigger();
if (rc.chan[2] > 0 && rc.chan[3] > 0)
adc_calibrate(ADC_CAL_GYRO);
return PITC_RESTART_TIMER;
}
if (rc.chan[2] > 0 && rc.chan[3] < 0) {
adc_calibrate(ADC_CAL_ACC);
static struct pitc_timer pitc_test_timer = {
.interval = 100,
.func = &pitc_test,
};
stick_cal_count = 3000;
global_state |= STICK_CALIBRATION;
rcontrol_calibrate(RC_CAL_START);
}
}
static void motor_mixer(int16_t pitch, int16_t nick, int16_t roll, int16_t yaw)
{
static uint8_t pwm[4];
rcontrol_getvalues(&rc);
pwm[0] = pitch + nick + yaw;
pwm[1] = pitch - nick + yaw;
pwm[2] = pitch + roll - yaw;
pwm[3] = pitch - roll - yaw;
static uint8_t pwm[4];
if (global_state & MOTORS_RUNNING) {
int32_t pitch = rc.chan[2] * 256;
int32_t gier = rc.chan[3] * 64;
int32_t nick = rc.chan[0] * 64;
int32_t roll = rc.chan[1] * 64;
int32_t data[4];
data[0] = (pitch - nick + gier) / 1024;
data[1] = (pitch + nick + gier) / 1024;
data[2] = (pitch + roll - gier) / 1024;
data[3] = (pitch - roll - gier) / 1024;
pwm[0] = (data[0] > 0x0f) ? data[0] : 0x0f;
pwm[1] = (data[1] > 0x0f) ? data[1] : 0x0f;
pwm[2] = (data[2] > 0x0f) ? data[2] : 0x0f;
pwm[3] = (data[3] > 0x0f) ? data[3] : 0x0f;
} else {
pwm[0] = 0x00;
pwm[1] = 0x00;
pwm[2] = 0x00;
pwm[3] = 0x00;
}
// printf("%3d %3d %3d %3d\n\r", pwm[0], pwm[1], pwm[2], pwm[3]);
twi_setpwm(pwm);
// adc_trigger();
return PITC_RESTART_TIMER;
}
static struct pitc_timer stick_timer = {
.interval = 10,
.func = &stick_timer_cb,
};
int main(void)
{
/* LED outputs */
@@ -113,8 +149,7 @@ int main(void)
/* adc, need timer */
at91_adc_init();
pitc_schedule_timer(&pitc_test_timer);
pitc_schedule_timer(&stick_timer);
// rcontrol_calibrate(RC_CAL_START);
while (1);
}

+ 29
- 35
src/at91_adc.c View File

@@ -77,8 +77,33 @@ static uint32_t adc_calibrate_cb(struct pitc_timer *timer)
adc_cal_data[1] /= ADC_CAL_COUNT;
adc_cal_data[2] /= ADC_CAL_COUNT;

adc_cal_mode = 0;
if (adc_cal_mode == ADC_CAL_GYRO) {
adc_offset[ADC_GYRO_ROLL] = adc_cal_data[0];
adc_offset[ADC_GYRO_NICK] = adc_cal_data[1];
adc_offset[ADC_GYRO_YAW] = adc_cal_data[2];

// TODO: check for invalid values (not centered) -> return value?

printf("GYRO offsets: %4d/%4d/%4d\n\r",
adc_offset[ADC_GYRO_ROLL],
adc_offset[ADC_GYRO_NICK],
adc_offset[ADC_GYRO_YAW]);

} else {
adc_offset[ADC_ACC_ROLL] = adc_cal_data[0];
adc_offset[ADC_ACC_NICK] = adc_cal_data[1];

// TODO: only 1/2 offset?
adc_offset[ADC_ACC_YAW] = adc_cal_data[2];

// TODO: save to eeprom
printf("ACC offsets: %4d/%4d/%4d\n\r",
adc_offset[ADC_ACC_ROLL],
adc_offset[ADC_ACC_NICK],
adc_offset[ADC_ACC_YAW]);
}

adc_cal_mode = ADC_CAL_NONE;
return PITC_REMOVE_TIMER;
}

@@ -103,43 +128,16 @@ void adc_trigger(void)

void adc_calibrate(uint32_t mode)
{
if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) {
if (adc_cal_mode != ADC_CAL_NONE)
return;

if (mode == ADC_CAL_GYRO || mode == ADC_CAL_ACC) {
adc_cal_count = ADC_CAL_COUNT;
adc_cal_mode = mode;
adc_cal_data[0] = 0;
adc_cal_data[1] = 0;
adc_cal_data[2] = 0;
pitc_schedule_timer(&adc_cal_timer);

// wait (1024 * 1ms ~> 1s)
while (adc_cal_mode);

if (mode == ADC_CAL_GYRO) {
adc_offset[ADC_GYRO_ROLL] = adc_cal_data[0];
adc_offset[ADC_GYRO_NICK] = adc_cal_data[1];
adc_offset[ADC_GYRO_YAW] = adc_cal_data[2];

// TODO: check for invalid values (not centered) -> return value?

printf("GYRO offsets: %4d/%4d/%4d\n\r",
adc_offset[ADC_GYRO_ROLL],
adc_offset[ADC_GYRO_NICK],
adc_offset[ADC_GYRO_YAW]);

} else {
adc_offset[ADC_ACC_ROLL] = adc_cal_data[0];
adc_offset[ADC_ACC_NICK] = adc_cal_data[1];

// TODO: only 1/2 offset?
adc_offset[ADC_ACC_YAW] = adc_cal_data[2];

// TODO: save to eeprom
printf("ACC offsets: %4d/%4d/%4d\n\r",
adc_offset[ADC_ACC_ROLL],
adc_offset[ADC_ACC_NICK],
adc_offset[ADC_ACC_YAW]);
}
}
}

@@ -183,8 +181,4 @@ void at91_adc_init(void)
aic->AIC_SMR[AT91C_ID_ADC] = IRQPRIO_ADC | AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL;
aic->AIC_SVR[AT91C_ID_ADC] = (uint32_t)at91_adc_isr;
aic->AIC_IECR = (1<<AT91C_ID_ADC);

// TODO: trigger via rc
adc_calibrate(ADC_CAL_GYRO);
adc_calibrate(ADC_CAL_ACC);
}

+ 23
- 0
src/at91_tc1.c View File

@@ -27,7 +27,9 @@
#define PULSE_MIN 0x0500
#define PULSE_MAX 0x0D00
#define PULSE_TIMEOUT 0x0F00

#define PULSE_CENTER 0x08C0
#define PULSE_SWITCH 0x01F0

/* moving average filters */
#define PULSE_FILTER_FAST (1<<2)
@@ -132,11 +134,32 @@ uint32_t rcontrol_getvalues(struct rc_values *rc)
if (tmp < -VALUE_RANGE)
tmp = -VALUE_RANGE;

// TODO: stick mapping from eeprom
rc->chan[i] = tmp;
}
return cnt;
}

uint32_t rcontrol_getswitches(struct rc_values *rc)
{
if (valid < 5)
return 0;

uint32_t i;
uint32_t cnt = MIN(ROUND_DIV256(count), ARRAY_SIZE(ch_data));
for (i = 0; i < cnt; i++) {
if (ch_data[i].width > (PULSE_CENTER + PULSE_SWITCH))
rc->chan[i] = VALUE_RANGE;

else if (ch_data[i].width < (PULSE_CENTER - PULSE_SWITCH))
rc->chan[i] = -VALUE_RANGE;

else
rc->chan[i] = 0;
}
return cnt;
}

void rcontrol_calibrate(uint32_t mode)
{
uint32_t i;


Loading…
Cancel
Save