CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/Blimp/motors.cpp
Views: 1798
1
#include "Blimp.h"
2
3
#define ARM_DELAY 20 // called at 10hz so 2 seconds
4
#define DISARM_DELAY 20 // called at 10hz so 2 seconds
5
#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds
6
#define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second
7
8
// arm_motors_check - checks for pilot input to arm or disarm the blimp
9
// called at 10hz
10
void Blimp::arm_motors_check()
11
{
12
static int16_t arming_counter;
13
14
// check if arming/disarm using rudder is allowed
15
AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type();
16
if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {
17
return;
18
}
19
20
// ensure throttle is down
21
if (channel_up->get_control_in() > 0) { //MIR what dow we do with this?
22
arming_counter = 0;
23
return;
24
}
25
26
int16_t yaw_in = channel_yaw->get_control_in();
27
28
// full right
29
if (yaw_in > 900) {
30
31
// increase the arming counter to a maximum of 1 beyond the auto trim counter
32
if (arming_counter <= AUTO_TRIM_DELAY) {
33
arming_counter++;
34
}
35
36
// arm the motors and configure for flight
37
if (arming_counter == ARM_DELAY && !motors->armed()) {
38
// reset arming counter if arming fail
39
if (!arming.arm(AP_Arming::Method::RUDDER)) {
40
arming_counter = 0;
41
}
42
}
43
44
// arm the motors and configure for flight
45
if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && control_mode == Mode::Number::MANUAL) {
46
gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start");
47
auto_trim_counter = 250;
48
auto_trim_started = false;
49
}
50
51
// full left and rudder disarming is enabled
52
} else if ((yaw_in < -900) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) {
53
if (!flightmode->has_manual_throttle() && !ap.land_complete) {
54
arming_counter = 0;
55
return;
56
}
57
58
// increase the counter to a maximum of 1 beyond the disarm delay
59
if (arming_counter <= DISARM_DELAY) {
60
arming_counter++;
61
}
62
63
// disarm the motors
64
if (arming_counter == DISARM_DELAY && motors->armed()) {
65
arming.disarm(AP_Arming::Method::RUDDER);
66
}
67
68
// Yaw is centered so reset arming counter
69
} else {
70
arming_counter = 0;
71
}
72
}
73
74
75
// motors_output - send output to motors library which will adjust and send to ESCs and servos
76
void Blimp::motors_output()
77
{
78
// output any servo channels
79
SRV_Channels::calc_pwm();
80
81
auto &srv = AP::srv();
82
83
// cork now, so that all channel outputs happen at once
84
srv.cork();
85
86
// update output on any aux channels, for manual passthru
87
SRV_Channels::output_ch_all();
88
89
// send output signals to motors
90
motors->output();
91
92
// push all channels
93
srv.push();
94
}
95
96