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

Lots of development of new features. Radio input, motor output, PID control

Browse Source 
loops, boot-time initialisation, option to run without attached UART.

And.. it flies!
This commit is contained in:
Gavan Fantom
2012-08-30 12:42:38 +00:00
parent 2890007195
commit 3f12132231
18 changed files with 727 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
wmp.c
View File

@@ -1,9 +1,11 @@
/* wmp.c */
#include "wmp.h"
#include "i2c.h"
#include "uart.h"
#include "dcm.h"
#include "fisqrt.h"
#include "stick.h"
#define WMP_ZERO_COUNT 100
@@ -115,6 +117,7 @@ int accel_z;
bool wmp_update;
bool wmp_zero;
unsigned int wmp_discard;
#define TWO_PI 6.28318531f
#define DEG_TO_RAD (TWO_PI/360.0f)
@@ -201,24 +204,28 @@ void wmp_process_gyro_sample(void)
wmp_generation++;
#if 1
if ((wmp_generation % 2) == 0)
#if SEND_DCM
if ((wmp_generation % 20) == 0)
dcm_send_packet();
#endif
} else if (wmp_zero) {
wmp_yaw_zero += wmp_yaw;
wmp_pitch_zero += wmp_pitch;
wmp_roll_zero += wmp_roll;
wmp_generation++;
if (wmp_generation >= WMP_ZERO_COUNT) {
wmp_zero = FALSE;
wmp_update = TRUE;
wmp_generation = 0;
wmp_yaw_zero /= WMP_ZERO_COUNT;
wmp_pitch_zero /= WMP_ZERO_COUNT;
wmp_roll_zero /= WMP_ZERO_COUNT;
putstr("Zero finished\r\n");
if (wmp_discard) {
wmp_discard--;
} else {
wmp_yaw_zero += wmp_yaw;
wmp_pitch_zero += wmp_pitch;
wmp_roll_zero += wmp_roll;
wmp_generation++;
if (wmp_generation >= WMP_ZERO_COUNT) {
wmp_zero = FALSE;
wmp_update = TRUE;
wmp_generation = 0;
wmp_yaw_zero /= WMP_ZERO_COUNT;
wmp_pitch_zero /= WMP_ZERO_COUNT;
wmp_roll_zero /= WMP_ZERO_COUNT;
putstr("Zero finished\r\n");
}
}
}
}
@@ -268,6 +275,7 @@ void wmp_process_accel_sample(void)
* on that so we'll just fudge it here.
*/
dcm_drift_correction(x, -y, -z);
stick_input();
}
void wmp_event_handler(void)
@@ -287,6 +295,7 @@ void wmp_start_zero(void)
{
wmp_zero = TRUE;
wmp_update = FALSE;
wmp_discard = 100;
wmp_generation = 0;
putstr("Starting zero\r\n");
}