philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Remove square root from magnitude calculation on accel_gyro.c

Browse Source 
Motors start now at 990 instead of 1000 to prevent glitches
This commit is contained in:
philsson 2016-11-03 11:58:51 +01:00
parent 8bd9dc986d
commit 3eeb4d3741
2 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
UAV-ControlSystem/src/drivers/accel_gyro.c
View File

@ -245,6 +245,8 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
HAL_Delay(60);
accel->pitchAngle = 0;
accel->rollAngle = 0;
return true;
}
@ -558,14 +560,15 @@ void mpu6000_read_angle(accel_t* accel, gyro_t* gyro)
accel->accelZconv = smooth[2] * smooth[2] * sign[2];
/* The total magnitude of the acceleration */
float magnitude = sqrtf( \
float magnitude = ( \
ABS_FLOAT(accel->accelXconv) * ABS_FLOAT(accel->accelXconv) + \
ABS_FLOAT(accel->accelYconv) * ABS_FLOAT(accel->accelYconv) + \
ABS_FLOAT(accel->accelZconv) * ABS_FLOAT(accel->accelZconv) \
);
/* Everything is normal. No outer forces */
if (0.85 < magnitude && magnitude < 1.15)
if (0.81 < magnitude && magnitude < 1.21)
//if (false)
{
// Roll
accel->rollAngle = -atan2(accel->accelXconv, sqrt(accel->accelYconv*accel->accelYconv + accel->accelZconv*accel->accelZconv))*180/M_PI;

7
UAV-ControlSystem/src/drivers/motors.c
View File

@ -14,6 +14,7 @@
#include "drivers/motors.h"
#include "drivers/failsafe_toggles.h"
#include "config/eeprom.h"
#include "drivers/motormix.h"
const int MotorPWMPeriode = 2000; //Micro seconds
const int MotorPWMInitPulse = 1000;
@ -162,7 +163,11 @@ void pwmEnableMotor(uint8_t motor, motorOutput motorOutput)
**************************************************************************/
void pwmEnableAllMotors(motorOutput motorOutput)
{
for (uint8_t i = 1; i < 11; i++ ) pwmEnableMotor(i, motorOutput);
for (uint8_t i = 1; i < 11; i++ )
{
pwmEnableMotor(i, motorOutput);
pwmAdjustSpeedOfMotor(i,mixerConfig.minCommand);
}
}
/**************************************************************************