CSU Chico Tilt Rotor Flying Machine, (Programming and CPU, 3 of 3). / Computer Science / Forums

Forums

4hv.org :: Forums :: Computer Science

« Previous topic | Next topic »

CSU Chico Tilt Rotor Flying Machine, (Programming and CPU, 3 of 3).

1 2 3 4 next last

Move Thread

LAN_403

Patrick

Thu Mar 29 2012, 01:09AM

Registered Member #2431 Joined: Tue Oct 13 2009, 09:47PM
Location: Chico, CA. USA
Posts: 5639

CSU Chico Tilt Rotor Flying Machine, (Programming and CPU, 3 of 3).

Related Web Sources:
Servo control using STM32VLDiscovery

I have 2 ESC's and 2 servos, all require the normal servo PWM. the ESC's at 50 Hz PRF, the servos at 300HZ PRF.

code from the above link im trying to figure out:
+++++++++++++++++++++++++++++++++++++++++++++++++ +++

// STM32 4-channel Servo Demo for 24 MHz VL Discovery - **link**

#include "stm32F10x.h"
#include "STM32vldiscovery.h"

/ ************************************************** ************************************/

void RCC_Configuration(void);
void GPIO_Configuration(void);
void NVIC_Configuration(void);
void TIM3_Configuration(void);

/ ************************************************** ************************************/

volatile int servo_angle[4] = { 0, 0, 0, 0 }; // +/- 90 degrees, use floats for fractional

void TIM3_IRQHandler(void)
{
if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_Update);

// minimum high of 600 us for -90 degrees, with +90 degrees at 2400 us, 10 us per degree
// timer timebase set to us units to simplify the configuration/math

TIM3->CCR1 = 600 + ((servo_angle[0] + 90) * 10); // where angle is an int -90 to +90 degrees, PC.6
TIM3->CCR2 = 600 + ((servo_angle[1] + 90) * 10); // where angle is an int -90 to +90 degrees, PC.7
TIM3->CCR3 = 600 + ((servo_angle[2] + 90) * 10); // where angle is an int -90 to +90 degrees, PC.8
TIM3->CCR4 = 600 + ((servo_angle[3] + 90) * 10); // where angle is an int -90 to +90 degrees, PC.9
}
}

/ ************************************************** ************************************/

int main(void)
{
RCC_Configuration();

GPIO_Configuration();

NVIC_Configuration();

TIM3_Configuration();

while(1)
{
// Insert code here to stroke your servos
}
}

/ ************************************************** ************************************/

void RCC_Configuration(void)
{
// clock for GPIO and AFIO (Remap)

RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO, ENABLE);

// clock for TIM3

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
}

/ ************************************************** ************************************/

void NVIC_Configuration(void)
{
NVIC_InitTypeDef NVIC_InitStructure;

/* Configure the NVIC Preemption Priority Bits */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0);

/* Enable the TIM3 Interrupt */
NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}

/ ************************************************** ************************************/

void GPIO_Configuration(void)
{
GPIO_InitTypeDef GPIO_InitStructure;

// PC.06 TIM3_CH1
// PC.07 TIM3_CH2
// PC.08 TIM3_CH3
// PC.09 TIM3_CH4

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;

GPIO_Init(GPIOC, &GPIO_InitStructure);

GPIO_PinRemapConfig(GPIO_FullRemap_TIM3, ENABLE); // Remap to get signal on PC.06-09
}

/ ************************************************** ************************************/

void TIM3_Configuration(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;

// Simplify the math here so TIM_Pulse has 1 us units

// Use (24-1) for VL @ 24 MHz
// Use (72-1) for STM32 @ 72 MHz

TIM_TimeBaseStructure.TIM_Prescaler = 24 - 1; // 24 MHz / 24 = 1 MHz

TIM_TimeBaseStructure.TIM_Period = 20000 - 1; // 1 MHz / 20000 = 50 Hz (20 ms)

TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;

TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;

TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

// Set up 4 channel servo

TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;

TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;

TIM_OCInitStructure.TIM_Pulse = 600 + 900; // 1500 us - Servo Top Centre

TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;

TIM_OC1Init(TIM3, &TIM_OCInitStructure); // Channel 1 configuration = PC.06 TIM3_CH1
TIM_OC2Init(TIM3, &TIM_OCInitStructure); // Channel 2 configuration = PC.07 TIM3_CH2
TIM_OC3Init(TIM3, &TIM_OCInitStructure); // Channel 3 configuration = PC.08 TIM3_CH3
TIM_OC4Init(TIM3, &TIM_OCInitStructure); // Channel 4 configuration = PC.09 TIM3_CH4

// turning on TIM3 and PWM outputs

TIM_Cmd(TIM3, ENABLE);

TIM_CtrlPWMOutputs(TIM3, ENABLE);

// TIM IT enable
TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
}

++++++++++++++++++++++++++++++++++++++++++++++++ +++++++++++++

more code shortly...

Patrick

Fri Mar 30 2012, 04:23AM

Registered Member #2431 Joined: Tue Oct 13 2009, 09:47PM
Location: Chico, CA. USA
Posts: 5639

PID loops and Kalman filters...

ok ive found the "PID without a PhD" PDF, and other sources:

Improving the Beginnerâ€™s PID

"PID Without A PhD" (Pdf)

feed forward and velocity, acceleration.

Patrick

Thu Apr 19 2012, 07:16AM

Registered Member #2431 Joined: Tue Oct 13 2009, 09:47PM
Location: Chico, CA. USA
Posts: 5639

the Discovery boards:

Patrick

Tue Jul 10 2012, 07:38PM

Registered Member #2431 Joined: Tue Oct 13 2009, 09:47PM
Location: Chico, CA. USA
Posts: 5639

OK heres the list:

-- lets just talk about the pitch axis for simplicity, the other axis' will be parallel and similar anyway.
-- lets define the pitch axis as a "Y" axis, and nose up means positive above zero, while nose down means negative degrees below zero.

-- there are gyros in the kk board, but i dont have access to them or thier code.
-- i want to seperate navigation and stability, focusing only on stability for now. i want it to hover, well do more later
-- Ill keep my gyros 10 times slower than the kk's gyros, to avoid aliasing and or oscillating feedback.
-- ill post a schematic below of the control flow

AndrewM

Tue Jul 10 2012, 08:34PM

Registered Member #49 Joined: Thu Feb 09 2006, 04:05AM
Location: Bigass Pile of Penguins
Posts: 362

I'm not going to look up the specs for all these board, why don't you make a simple post regarding your plan?

What is a KK board. Its expecting servo PWM input... so this must be some kind of off the shelf gyro?

If that board isn't controlling your stability adequate... why are you using it? I'm not sure I follow the logic of having a micro running a PID controller... feeding into an off the shelf PID controller.

Patrick

Tue Jul 10 2012, 08:39PM

Registered Member #2431 Joined: Tue Oct 13 2009, 09:47PM
Location: Chico, CA. USA
Posts: 5639

AndrewM wrote ...

I'm not going to look up the specs for all these board, why don't you make a simple post regarding your plan?

i wasnt expecting anyone to do so.

AndrewM wrote ...

What is a KK board. Its expecting servo PWM input... so this must be some kind of off the shelf gyro?

If that board isn't controlling your stability adequate... why are you using it? I'm not sure I follow the logic of having a micro running a PID controller... feeding into an off the shelf PID controller.

well it doesnt seem to be worth a dam, we can get rid of it. all it does is take in a RC control from the human, pass it to a gyro stability program and spilt up that result to the serovs and fans. Minsoo kim didnt put accelerometers on it so it cant hold it self level. for 240-350$ more i guess i could buy his lame upgrade, the black board which im using know cost me 140$ already.

what id like to do, since he won t answer my emails, is just to release an open source code here at 4hv that anyone could load on an STM32 board for $9-$25 then go fly. then yould need some spark fun orsimilar supplier of the imu, but still its cheaper and better.

AndrewM

Tue Jul 10 2012, 09:31PM

Registered Member #49 Joined: Thu Feb 09 2006, 04:05AM
Location: Bigass Pile of Penguins
Posts: 362

Yeah thats what I thought. I have a similar unit for fixed wing aircraft and it works pretty well... but I'd be pretty wary about dropping it into something like this. From your videos its just not up to snuff so I don't think adding another PID layer upstream is going to be a good solution.

So you basic sensor suite is... 3 axis gyro and 3 axis accel? And all of this is accessible by your STM32?

I think your basic case should be a stable hover... that is, all rates are zero and the angle of the gravity vector equals your user's commanded angle (ie. the stick position). You said in your other thread you want to control Pitch first (indeed I think that will be your toughest axis to control). So lets call pitch "Î¸"...

Proportional error:
E_p = Î¸_dot_gyro

Integral error:
E_i = Î¸_actual - Î¸_commanded

The derivative term is harder since you don't have a derivative sensor. You'll want to make sure you have nice clean data coming out of your rate sensor, otherwise your derivative terms can go nuts; if you can't clean the data then you'll want to implement a running average or something, depending on your sample rate.

To be honest I'd try PI control first... because I bet that even if you DO implement D control that it will be so noisy you'll need to turn the gain down to uselessness.

Then your loop output would be:

u = Ki * E_i + Kp * E_p

You'll need to write your equations of motion in order to determine how to use the u signal... but almost surely you'll want u to signal the motion of your fan angle servos together (ie. left and right side equally).

Patrick

Wed Jul 11 2012, 12:11AM

Registered Member #2431 Joined: Tue Oct 13 2009, 09:47PM
Location: Chico, CA. USA
Posts: 5639

AndrewM wrote ...

Yeah thats what I thought. I have a similar unit for fixed wing aircraft and it works pretty well... but I'd be pretty wary about dropping it into something like this. From your videos its just not up to snuff so I don't think adding another PID layer upstream is going to be a good solution.

yep we'll ditch the KKmulticopter board, and move on.

AndrewM wrote ...

So you basic sensor suite is... 3 axis gyro and 3 axis accel? And all of this is accessible by your STM32?

yep, i can access the gyro at 400 or 800 hz, and the accel at 400 to 2000 dps... this imu is fully under my control and coded right to the STM32.

AndrewM wrote ...

I think your basic case should be a stable hover... that is, all rates are zero and the angle of the gravity vector equals your user's commanded angle (ie. the stick position). You said in your other thread you want to control Pitch first (indeed I think that will be your toughest axis to control). So lets call pitch "Î¸"...

Proportional error:
E_p = Î¸_dot_gyro

Integral error:
E_i = Î¸_actual - Î¸_commanded

ok, my brain is slowing...

AndrewM wrote ...

The derivative term is harder since you don't have a derivative sensor. You'll want to make sure you have nice clean data coming out of your rate sensor, otherwise your derivative terms can go nuts; if you can't clean the data then you'll want to implement a running average or something, depending on your sample rate.

To be honest I'd try PI control first... because I bet that even if you DO implement D control that it will be so noisy you'll need to turn the gain down to uselessness.

ok, well we can leave out the D term for now, but i was going to use a complimentary filter as suggested by big5824, to "de-noise" the signal.

big5824's site :

raw data

filtered with complimetary scheme

AndrewM wrote ...

Then your loop output would be:

u = Ki * E_i + Kp * E_p

You'll need to write your equations of motion in order to determine how to use the u signal... but almost surely you'll want u to signal the motion of your fan angle servos together (ie. left and right side equally).

ok i see the above as a simple PI type deal, i can figure out how to code that, but what do you mean by "You'll need to write your equations of motion ..."

AndrewM

Wed Jul 11 2012, 12:40AM

Registered Member #49 Joined: Thu Feb 09 2006, 04:05AM
Location: Bigass Pile of Penguins
Posts: 362

Patrick wrote ...

ok i see the above as a simple PI type deal, i can figure out how to code that, but what do you mean by "You'll need to write your equations of motion ..."

Write equations for the pitch rate, Î¸_dot, in terms of the fan state.

For a PI controller it'll be simple, like:

Î¸_dot = F_fan * distance_to_c.m. / I_pitch

distance_to_c.m. will be a function of the fan servo angle... when you write that equation you should see immediately why steve and I told you to get your fans as far above the center of mass as possible.

When you have that equation you can model the system and simulate how to utilize the output of the loop, u.

Eventually when you implement a more complex controller you can add things to the EOM, like the acceleration of the fan servos; changing the fan angle will jerk the body of your craft around, for example. You can make the EOMs as complex as you like.

Also: keep in mind that you'll need a solution to correct your gyro drift - this is apparently where people have implemented kalman filters and the like. I haven't ever built a control loop like this so I don't know if its a necessity or a "nice-to-have." Seems to me that, as a first order attempt, a simple high pass filter on the gyro output before the controller should be enough to get things working...

Patrick

Wed Jul 11 2012, 12:56AM

Registered Member #2431 Joined: Tue Oct 13 2009, 09:47PM
Location: Chico, CA. USA
Posts: 5639

AndrewM wrote ...

Patrick wrote ...

ok i see the above as a simple PI type deal, i can figure out how to code that, but what do you mean by "You'll need to write your equations of motion ..."

ok i see what youre getting at with the equation now.

are you meaning dot product for theta_dot?
F = newtons
dist = meters
what units are I_pitch? is that an angle?

a projection of one onto the other?

1 2 3 4 next last

Moderator(s): Chris Russell, Noelle, Alex, Tesladownunder, Dave Marshall, Dave Billington, Bjørn, Steve Conner, Wolfram, Kizmo, Mads Barnkob