Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/Blimp/motors.cpp
Views: 1798
#include "Blimp.h"12#define ARM_DELAY 20 // called at 10hz so 2 seconds3#define DISARM_DELAY 20 // called at 10hz so 2 seconds4#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds5#define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second67// arm_motors_check - checks for pilot input to arm or disarm the blimp8// called at 10hz9void Blimp::arm_motors_check()10{11static int16_t arming_counter;1213// check if arming/disarm using rudder is allowed14AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type();15if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {16return;17}1819// ensure throttle is down20if (channel_up->get_control_in() > 0) { //MIR what dow we do with this?21arming_counter = 0;22return;23}2425int16_t yaw_in = channel_yaw->get_control_in();2627// full right28if (yaw_in > 900) {2930// increase the arming counter to a maximum of 1 beyond the auto trim counter31if (arming_counter <= AUTO_TRIM_DELAY) {32arming_counter++;33}3435// arm the motors and configure for flight36if (arming_counter == ARM_DELAY && !motors->armed()) {37// reset arming counter if arming fail38if (!arming.arm(AP_Arming::Method::RUDDER)) {39arming_counter = 0;40}41}4243// arm the motors and configure for flight44if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && control_mode == Mode::Number::MANUAL) {45gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start");46auto_trim_counter = 250;47auto_trim_started = false;48}4950// full left and rudder disarming is enabled51} else if ((yaw_in < -900) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) {52if (!flightmode->has_manual_throttle() && !ap.land_complete) {53arming_counter = 0;54return;55}5657// increase the counter to a maximum of 1 beyond the disarm delay58if (arming_counter <= DISARM_DELAY) {59arming_counter++;60}6162// disarm the motors63if (arming_counter == DISARM_DELAY && motors->armed()) {64arming.disarm(AP_Arming::Method::RUDDER);65}6667// Yaw is centered so reset arming counter68} else {69arming_counter = 0;70}71}727374// motors_output - send output to motors library which will adjust and send to ESCs and servos75void Blimp::motors_output()76{77// output any servo channels78SRV_Channels::calc_pwm();7980auto &srv = AP::srv();8182// cork now, so that all channel outputs happen at once83srv.cork();8485// update output on any aux channels, for manual passthru86SRV_Channels::output_ch_all();8788// send output signals to motors89motors->output();9091// push all channels92srv.push();93}949596