1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
|
#include "motormixer.h"
#include <ch.h>
#include <hal.h>
template<class T>
inline T abs(T val) {
if(val > 0) {
return val;
} else {
return -val;
}
}
namespace {
uint16_t motors[4];
void foo(PWMDriver*) {
pwmEnableChannel(&PWMD2, 0, 1000 + motors[0]);
pwmEnableChannel(&PWMD2, 1, 1000 + motors[1]);
pwmEnableChannel(&PWMD2, 2, 1000 + motors[2]);
pwmEnableChannel(&PWMD2, 3, 1000 + motors[3]);
}
static PWMConfig pwmcfg = {
1000000,
1000000 / 50,
foo,
{
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
{PWM_OUTPUT_ACTIVE_HIGH, NULL},
{PWM_OUTPUT_ACTIVE_HIGH, NULL}
},
0
};
};
void MotorMixer::start() {
pwmStart(&PWMD2, &pwmcfg);
palSetPadMode(GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetPadMode(GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetPadMode(GPIOA, 2, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
palSetPadMode(GPIOA, 3, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
pwmEnableChannel(&PWMD2, 0, 1000);
pwmEnableChannel(&PWMD2, 1, 1000);
pwmEnableChannel(&PWMD2, 2, 1000);
pwmEnableChannel(&PWMD2, 3, 1000);
}
void MotorMixer::update(int16_t thrust, int16_t pitch, int16_t roll, int16_t yaw) {
//thrust = ppmsum.data[2];
if(thrust < 0) {
thrust = 0;
}
if(thrust > 1000) {
thrust = 1000;
}
// Check that total thrust will not be exceeded.
if(abs(pitch) + abs(roll) > thrust) {
// If saturated by pitch/roll, drop yaw component.
yaw = 0;
// Scale pitch and roll.
int32_t df = ((abs(pitch) + abs(roll)) << 16) / thrust;
pitch = (pitch << 16) / df;
roll = (roll << 16) / df;
} else if(abs(pitch) + abs(roll) + abs(yaw) > thrust) {
// Scale yaw value.
yaw = (yaw > 0 ? 1 : -1) * (thrust - abs(pitch) - abs(roll));
}
motors[0] = thrust + pitch + roll - yaw;
motors[1] = thrust + pitch - roll + yaw;
motors[2] = thrust - pitch + roll + yaw;
motors[3] = thrust - pitch - roll - yaw;
//sensordata[20] = motor_1;
//sensordata[21] = motor_2;
//sensordata[22] = motor_3;
//sensordata[23] = motor_4;
}
|
