coolfactor/quadrotor
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Add missing file and tidy up somewhat.

Browse Source
This commit is contained in:
Gavan Fantom
2014-11-23 20:20:48 +00:00
parent 8b3ff9feba
commit 4f171f3023
12 changed files with 136 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
motor.c
View File

@@ -27,10 +27,12 @@ float throttle = 0.0f;
* Perform a PID loop iteration.
* roll and pitch are absolute values
* yaw is, currently, a rate.
* For this function only, coordinate convention is:
* x = roll
* y = pitch
* z = yaw
*/
void motor_pid_update(float troll, float mroll,
float tpitch, float mpitch,
float tyaw, float myaw)
void motor_pid_update(vec3f target, vec3f measured)
{
float derivative[3];
float out[3];
@@ -40,16 +42,16 @@ void motor_pid_update(float troll, float mroll,
float min_motor;
int i;
roll = troll - mroll;
pitch = tpitch - mpitch;
yaw = tyaw - myaw;
roll = target.x - measured.x;
pitch = target.y - measured.y;
yaw = target.z - measured.z;
#if 0
if ((stick_counter % 100) == 0) {
putstr("{");
putint_s((int)(tyaw * 10000));
putint_s((int)(target.z * 10000));
putstr(", ");
putint_s((int)(myaw * 10000));
putint_s((int)(measured.z * 10000));
putstr("}\r\n");
}
#endif
@@ -67,13 +69,13 @@ void motor_pid_update(float troll, float mroll,
}
/* The measurements are the opposite sign to the error */
derivative[0] = (-mroll - last[0]) / delta_t;
derivative[1] = (-mpitch - last[1]) / delta_t;
derivative[2] = (-myaw - last[2]) / delta_t;
derivative[0] = (-measured.x - last[0]) / delta_t;
derivative[1] = (-measured.y - last[1]) / delta_t;
derivative[2] = (-measured.z - last[2]) / delta_t;
last[0] = -mroll;
last[1] = -mpitch;
last[2] = -myaw;
last[0] = -measured.x;
last[1] = -measured.y;
last[2] = -measured.z;
out[0] = roll * Kp + integral[0] * Ki + derivative[0] * Kd;
out[1] = pitch * Kp + integral[1] * Ki + derivative[1] * Kd;