mirror of https://github.com/ArduPilot/ardupilot
Merge branch 'master' of https://code.google.com/p/ardupilot-mega
This commit is contained in:
commit
80a342fbf8
|
@ -1,6 +1,6 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#define THISFIRMWARE "ArduCopter V2.1.1r8 alpha"
|
||||
#define THISFIRMWARE "ArduCopter V2.1.1r9 alpha"
|
||||
/*
|
||||
ArduCopter Version 2.0 Beta
|
||||
Authors: Jason Short
|
||||
|
@ -80,6 +80,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
|
|||
// Configuration
|
||||
#include "defines.h"
|
||||
#include "config.h"
|
||||
#include "config_channels.h"
|
||||
|
||||
// Local modules
|
||||
#include "Parameters.h"
|
||||
|
@ -278,7 +279,6 @@ ModeFilter sonar_mode_filter;
|
|||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Global variables
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
static const char *comma = ",";
|
||||
|
||||
static const char* flight_mode_strings[] = {
|
||||
"STABILIZE",
|
||||
|
@ -327,7 +327,7 @@ static int16_t y_rate_error;
|
|||
////////////////////////////////////////////////////////////////////////////////
|
||||
// This is the state of the flight control system
|
||||
// There are multiple states defined such as STABILIZE, ACRO,
|
||||
static byte control_mode = STABILIZE;
|
||||
static int8_t control_mode = STABILIZE;
|
||||
// This is the state of simple mode.
|
||||
// Set in the control_mode.pde file when the control switch is read
|
||||
static bool do_simple = false;
|
||||
|
@ -443,8 +443,6 @@ static int32_t target_bearing;
|
|||
// This is the angle from the copter to the "next_WP" location
|
||||
// with the addition of Crosstrack error in degrees * 100
|
||||
static int32_t nav_bearing;
|
||||
// This is the angle from the copter to the "home" location in degrees * 100
|
||||
static int32_t home_bearing;
|
||||
// Status of the Waypoint tracking mode. Options include:
|
||||
// NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE
|
||||
static byte wp_control;
|
||||
|
@ -572,7 +570,8 @@ static int32_t baro_alt;
|
|||
static int32_t old_baro_alt;
|
||||
// The climb_rate as reported by Baro in cm/s
|
||||
static int16_t baro_rate;
|
||||
|
||||
//
|
||||
static boolean reset_throttle_flag;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// flight modes
|
||||
|
@ -681,12 +680,23 @@ static int16_t nav_throttle; // 0-1000 for throttle control
|
|||
static uint32_t throttle_integrator;
|
||||
// This is a future value for replacing the throttle_cruise setup procedure. It's an average of throttle control
|
||||
// that is generated when the climb rate is within a certain threshold
|
||||
static float throttle_avg = THROTTLE_CRUISE;
|
||||
//static float throttle_avg = THROTTLE_CRUISE;
|
||||
// This is a flag used to trigger the updating of nav_throttle at 10hz
|
||||
static bool invalid_throttle;
|
||||
// Used to track the altitude offset for climbrate control
|
||||
static int32_t target_altitude;
|
||||
//static int32_t target_altitude;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Climb rate control
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Time when we intiated command in millis - used for controlling decent rate
|
||||
// The orginal altitude used to base our new altitude during decent
|
||||
static int32_t original_altitude;
|
||||
// Used to track the altitude offset for climbrate control
|
||||
static int32_t target_altitude;
|
||||
static uint32_t alt_change_timer;
|
||||
static int8_t alt_change_flag;
|
||||
static uint32_t alt_change;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Navigation Yaw control
|
||||
|
@ -737,18 +747,8 @@ static int16_t event_undo_value;
|
|||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Delay Mission Scripting Command
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
|
||||
static int32_t condition_start;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Auto Landing
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Time when we intiated command in millis - used for controlling decent rate
|
||||
static int32_t land_start;
|
||||
// The orginal altitude used to base our new altitude during decent
|
||||
static int32_t original_alt;
|
||||
|
||||
static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
|
||||
static uint32_t condition_start;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -794,6 +794,8 @@ static float dTnav;
|
|||
static int16_t superslow_loopCounter;
|
||||
// RTL Autoland Timer
|
||||
static uint32_t auto_land_timer;
|
||||
// disarms the copter while in Acro or Stabilzie mode after 30 seconds of no flight
|
||||
static uint8_t auto_disarming_counter;
|
||||
|
||||
|
||||
// Tracks if GPS is enabled based on statup routine
|
||||
|
@ -802,7 +804,7 @@ static bool GPS_enabled = false;
|
|||
// Set true if we have new PWM data to act on from the Radio
|
||||
static bool new_radio_frame;
|
||||
// Used to auto exit the in-flight leveler
|
||||
static int16_t auto_level_counter;
|
||||
static int16_t auto_level_counter;
|
||||
|
||||
// Reference to the AP relay object - APM1 only
|
||||
AP_Relay relay;
|
||||
|
@ -865,6 +867,7 @@ void loop()
|
|||
|
||||
counter_one_herz++;
|
||||
|
||||
// trgger our 1 hz loop
|
||||
if(counter_one_herz >= 50){
|
||||
super_slow_loop();
|
||||
counter_one_herz = 0;
|
||||
|
@ -1129,7 +1132,7 @@ static void fifty_hz_loop()
|
|||
#if FRAME_CONFIG == TRI_FRAME
|
||||
// servo Yaw
|
||||
g.rc_4.calc_pwm();
|
||||
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
|
||||
APM_RC.OutputCh(CH_TRI_YAW, g.rc_4.radio_out);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -1193,16 +1196,28 @@ static void slow_loop()
|
|||
default:
|
||||
slow_loopCounter = 0;
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
#define AUTO_ARMING_DELAY 60
|
||||
// 1Hz loop
|
||||
static void super_slow_loop()
|
||||
{
|
||||
if (g.log_bitmask & MASK_LOG_CUR)
|
||||
Log_Write_Current();
|
||||
|
||||
// this function disarms the copter if it has been sitting on the ground for any moment of time greater than 30s
|
||||
// but only of the control mode is manual
|
||||
if((control_mode <= ACRO) && (g.rc_3.control_in == 0)){
|
||||
auto_disarming_counter++;
|
||||
if(auto_disarming_counter == AUTO_ARMING_DELAY){
|
||||
init_disarm_motors();
|
||||
}else if (auto_disarming_counter > AUTO_ARMING_DELAY){
|
||||
auto_disarming_counter = AUTO_ARMING_DELAY + 1;
|
||||
}
|
||||
}else{
|
||||
auto_disarming_counter = 0;
|
||||
}
|
||||
gcs_send_message(MSG_HEARTBEAT);
|
||||
gcs_data_stream_send(1,3);
|
||||
// agmatthews - USERHOOKS
|
||||
|
@ -1558,18 +1573,27 @@ void update_throttle_mode(void)
|
|||
throttle_out = g.throttle_cruise + angle_boost + manual_boost;
|
||||
#endif
|
||||
|
||||
// reset next_WP.alt and don't go below 1 meter
|
||||
next_WP.alt = max(current_loc.alt, 100);
|
||||
//force a reset of the altitude change
|
||||
clear_new_altitude();
|
||||
|
||||
/*
|
||||
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \t manb: %d\n",
|
||||
int16_t iterm = g.pi_alt_hold.get_integrator();
|
||||
|
||||
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \t manb: %d, iterm %d\n",
|
||||
next_WP.alt,
|
||||
current_loc.alt,
|
||||
altitude_error,
|
||||
manual_boost);
|
||||
manual_boost,
|
||||
iterm);
|
||||
//*/
|
||||
reset_throttle_flag = true;
|
||||
|
||||
}else{
|
||||
if(reset_throttle_flag) {
|
||||
set_new_altitude(max(current_loc.alt, 100));
|
||||
reset_throttle_flag = false;
|
||||
}
|
||||
|
||||
// 10hz, don't run up i term
|
||||
if(invalid_throttle && motor_auto_armed == true){
|
||||
|
||||
|
@ -1582,14 +1606,13 @@ void update_throttle_mode(void)
|
|||
// clear the new data flag
|
||||
invalid_throttle = false;
|
||||
/*
|
||||
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d, \trate_int %d \n",
|
||||
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d\n",
|
||||
next_WP.alt,
|
||||
current_loc.alt,
|
||||
altitude_error,
|
||||
nav_throttle,
|
||||
(int16_t)g.pi_alt_hold.get_integrator(),
|
||||
(int16_t) g.pi_throttle.get_integrator());
|
||||
*/
|
||||
(int16_t)g.pi_alt_hold.get_integrator());
|
||||
//*/
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
|
@ -1637,23 +1660,26 @@ static void update_navigation()
|
|||
break;
|
||||
|
||||
case RTL:
|
||||
// We have reached Home
|
||||
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
|
||||
// if this value > 0, we are set to trigger auto_land after 30 seconds
|
||||
set_mode(LOITER);
|
||||
auto_land_timer = millis();
|
||||
break;
|
||||
}
|
||||
|
||||
}else if(current_loc.alt < (next_WP.alt - 300)){
|
||||
// don't navigate if we are below our target alt
|
||||
wp_control = LOITER_MODE;
|
||||
// We wait until we've reached out new altitude before coming home
|
||||
// Arg doesn't work, it
|
||||
//if(alt_change_flag != REACHED_ALT){
|
||||
// wp_control = NO_NAV_MODE;
|
||||
//}else{
|
||||
wp_control = WP_MODE;
|
||||
|
||||
}else{
|
||||
// calculates desired Yaw
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
update_auto_yaw();
|
||||
#endif
|
||||
|
||||
wp_control = WP_MODE;
|
||||
}
|
||||
//}
|
||||
|
||||
// calculates the desired Roll and Pitch
|
||||
update_nav_wp();
|
||||
|
@ -1689,12 +1715,9 @@ static void update_navigation()
|
|||
case LAND:
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
if(verify_land()) { // JLN fix for auto land in RTL
|
||||
set_mode(STABILIZE);
|
||||
} else {
|
||||
// calculates the desired Roll and Pitch
|
||||
update_nav_wp();
|
||||
}
|
||||
// verify land will override WP_control if we are below
|
||||
// a certain altitude
|
||||
verify_land();
|
||||
|
||||
// calculates the desired Roll and Pitch
|
||||
update_nav_wp();
|
||||
|
@ -1827,7 +1850,8 @@ static void update_altitude()
|
|||
scale = (sonar_alt - 400) / 200;
|
||||
scale = constrain(scale, 0, 1);
|
||||
|
||||
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
|
||||
// solve for a blended altitude
|
||||
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
|
||||
|
||||
// solve for a blended climb_rate
|
||||
climb_rate = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
|
||||
|
@ -1840,7 +1864,6 @@ static void update_altitude()
|
|||
}
|
||||
|
||||
}else{
|
||||
|
||||
// NO Sonar case
|
||||
current_loc.alt = baro_alt + home.alt;
|
||||
climb_rate = baro_rate;
|
||||
|
@ -1848,25 +1871,14 @@ static void update_altitude()
|
|||
|
||||
// manage bad data
|
||||
climb_rate = constrain(climb_rate, -300, 300);
|
||||
|
||||
// update the target altitude
|
||||
next_WP.alt = get_new_altitude();
|
||||
}
|
||||
|
||||
static void
|
||||
adjust_altitude()
|
||||
{
|
||||
/*
|
||||
// old vert control
|
||||
if(g.rc_3.control_in <= 200){
|
||||
next_WP.alt -= 1; // 1 meter per second
|
||||
next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location
|
||||
next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter
|
||||
//manual_boost = (g.rc_3.control_in == 0) ? -20 : 0;
|
||||
|
||||
}else if (g.rc_3.control_in > 700){
|
||||
next_WP.alt += 1; // 1 meter per second
|
||||
next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location
|
||||
//manual_boost = (g.rc_3.control_in == 800) ? 20 : 0;
|
||||
}*/
|
||||
|
||||
if(g.rc_3.control_in <= 180){
|
||||
// we remove 0 to 100 PWM from hover
|
||||
manual_boost = g.rc_3.control_in - 180;
|
||||
|
|
|
@ -170,9 +170,6 @@ get_nav_throttle(int32_t z_error)
|
|||
int16_t rate_error;
|
||||
int16_t output;
|
||||
|
||||
// XXX HACK, need a better way not to ramp this i term in large altitude changes.
|
||||
float dt = (abs(z_error) < 400) ? .1 : 0.0;
|
||||
|
||||
// limit error to prevent I term run up
|
||||
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
|
||||
|
||||
|
@ -180,7 +177,7 @@ get_nav_throttle(int32_t z_error)
|
|||
rate_error = g.pi_alt_hold.get_p(z_error); //_p = .85
|
||||
|
||||
// experiment to pipe iterm directly into the output
|
||||
int16_t iterm = g.pi_alt_hold.get_i(z_error, dt);
|
||||
int16_t iterm = g.pi_alt_hold.get_i(z_error, .1);
|
||||
|
||||
// calculate rate error
|
||||
rate_error = rate_error - climb_rate;
|
||||
|
@ -228,23 +225,37 @@ get_rate_yaw(int32_t target_rate)
|
|||
|
||||
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
|
||||
// Keeps outdated data out of our calculations
|
||||
static void reset_hold_I(void)
|
||||
{
|
||||
g.pi_loiter_lat.reset_I();
|
||||
g.pi_loiter_lon.reset_I();
|
||||
}
|
||||
//static void reset_hold_I(void)
|
||||
//{
|
||||
// g.pi_loiter_lat.reset_I();
|
||||
// g.pi_loiter_lon.reset_I();
|
||||
//}
|
||||
|
||||
// Keeps old data out of our calculation / logs
|
||||
static void reset_nav(void)
|
||||
{
|
||||
// forces us to update nav throttle
|
||||
invalid_throttle = true;
|
||||
nav_throttle = 0;
|
||||
|
||||
// always start Circle mode at same angle
|
||||
circle_angle = 0;
|
||||
|
||||
// We must be heading to a new WP, so XTrack must be 0
|
||||
crosstrack_error = 0;
|
||||
|
||||
// Will be set by new command
|
||||
target_bearing = 0;
|
||||
|
||||
// Will be set by new command
|
||||
wp_distance = 0;
|
||||
|
||||
// Will be set by new command, used by loiter
|
||||
long_error = 0;
|
||||
lat_error = 0;
|
||||
|
||||
// Will be set by new command, used by loiter
|
||||
next_WP.alt = 0;
|
||||
}
|
||||
|
||||
static void reset_rate_I()
|
||||
|
|
|
@ -51,8 +51,8 @@ camera_stabilization()
|
|||
g.rc_camera_pitch.calc_pwm();
|
||||
g.rc_camera_roll.calc_pwm();
|
||||
|
||||
APM_RC.OutputCh(CH_5, g.rc_camera_pitch.radio_out);
|
||||
APM_RC.OutputCh(CH_6, g.rc_camera_roll.radio_out);
|
||||
APM_RC.OutputCh(CH_CAM_PITCH, g.rc_camera_pitch.radio_out);
|
||||
APM_RC.OutputCh(CH_CAM_ROLL , g.rc_camera_roll.radio_out);
|
||||
//Serial.printf("c:%d\n", g.rc_camera_pitch.radio_out);
|
||||
}
|
||||
|
||||
|
|
|
@ -308,7 +308,7 @@ static void Log_Read_Raw()
|
|||
for (int y = 0; y < 6; y++) {
|
||||
logvar = (float)DataFlash.ReadLong() / t7;
|
||||
Serial.print(logvar);
|
||||
Serial.print(comma);
|
||||
Serial.print(",");
|
||||
}
|
||||
Serial.println(" ");
|
||||
}
|
||||
|
|
|
@ -147,9 +147,13 @@ static void set_next_WP(struct Location *wp)
|
|||
// ---------------------
|
||||
next_WP = *wp;
|
||||
|
||||
// used to control and limit the rate of climb - not used right now!
|
||||
// -----------------------------------------------------------------
|
||||
target_altitude = current_loc.alt;
|
||||
// used to control and limit the rate of climb
|
||||
// -------------------------------------------
|
||||
// We don't set next WP below 1m
|
||||
next_WP.alt = max(next_WP.alt, 100);
|
||||
|
||||
// Save new altitude so we can track it for climb_rate
|
||||
set_new_altitude(next_WP.alt);
|
||||
|
||||
// this is used to offset the shrinking longitude as we go towards the poles
|
||||
float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925;
|
||||
|
@ -162,6 +166,9 @@ static void set_next_WP(struct Location *wp)
|
|||
target_bearing = get_bearing(&prev_WP, &next_WP);
|
||||
nav_bearing = target_bearing;
|
||||
|
||||
// calc the location error:
|
||||
calc_location_error(&next_WP);
|
||||
|
||||
// to check if we have missed the WP
|
||||
// ---------------------------------
|
||||
original_target_bearing = target_bearing;
|
||||
|
|
|
@ -199,7 +199,7 @@ static void do_RTL(void)
|
|||
|
||||
//so we know where we are navigating from
|
||||
// --------------------------------------
|
||||
next_WP = current_loc;
|
||||
next_WP = current_loc;
|
||||
|
||||
// Loads WP from Memory
|
||||
// --------------------
|
||||
|
@ -265,19 +265,13 @@ static void do_land()
|
|||
{
|
||||
wp_control = LOITER_MODE;
|
||||
|
||||
//Serial.println("dlnd ");
|
||||
|
||||
// not really used right now, might be good for debugging
|
||||
land_complete = false;
|
||||
|
||||
// used to limit decent rate
|
||||
land_start = millis();
|
||||
|
||||
// used to limit decent rate
|
||||
original_alt = current_loc.alt;
|
||||
|
||||
// hold at our current location
|
||||
set_next_WP(¤t_loc);
|
||||
|
||||
// Set a new target altitude
|
||||
set_new_altitude(0);
|
||||
}
|
||||
|
||||
static void do_loiter_unlimited()
|
||||
|
@ -339,55 +333,44 @@ static bool verify_takeoff()
|
|||
return false;
|
||||
}
|
||||
// are we above our target altitude?
|
||||
return (current_loc.alt > next_WP.alt);
|
||||
//return (current_loc.alt > next_WP.alt);
|
||||
return (current_loc.alt > target_altitude);
|
||||
}
|
||||
|
||||
static bool verify_land()
|
||||
{
|
||||
|
||||
static int32_t old_alt = 0;
|
||||
static int16_t velocity_land = -1;
|
||||
|
||||
// land at .62 meter per second
|
||||
next_WP.alt = original_alt - ((millis() - land_start) / 32); // condition_value = our initial
|
||||
|
||||
if (old_alt == 0)
|
||||
// landing detector
|
||||
if ((current_loc.alt - home.alt) > 300){
|
||||
old_alt = current_loc.alt;
|
||||
|
||||
if (velocity_land == -1)
|
||||
velocity_land = 2000;
|
||||
|
||||
|
||||
if ((current_loc.alt - home.alt) < 300){
|
||||
}else{
|
||||
// a LP filter used to tell if we have landed
|
||||
// will drive to 0 if we are on the ground - maybe, the baro is noisy
|
||||
velocity_land = ((velocity_land * 7) + (old_alt - current_loc.alt)) / 8;
|
||||
int16_t delta = (old_alt - current_loc.alt) << 3;
|
||||
velocity_land = ((velocity_land * 7) + delta ) / 8;
|
||||
}
|
||||
|
||||
//Serial.printf("velocity_land %d \n", velocity_land);
|
||||
|
||||
// remenber altitude for climb_rate
|
||||
old_alt = current_loc.alt;
|
||||
|
||||
if ((current_loc.alt - home.alt) < 200){
|
||||
// don't bank to hold position
|
||||
wp_control = NO_NAV_MODE;
|
||||
|
||||
// Update by JLN for a safe AUTO landing
|
||||
manual_boost = -10;
|
||||
g.throttle_cruise += g.pi_alt_hold.get_integrator();
|
||||
g.pi_alt_hold.reset_I();
|
||||
g.pi_throttle.reset_I();
|
||||
}
|
||||
|
||||
if((current_loc.alt - home.alt) < 100 && velocity_land <= 100){
|
||||
if((current_loc.alt - home.alt) < 100 && velocity_land <= 50){
|
||||
land_complete = true;
|
||||
// reset manual_boost hack
|
||||
manual_boost = 0;
|
||||
|
||||
// reset old_alt
|
||||
old_alt = 0;
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
//Serial.printf("N, %d\n", velocity_land);
|
||||
//Serial.printf("N_alt, %ld\n", next_WP.alt);
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -396,7 +379,9 @@ static bool verify_nav_wp()
|
|||
// Altitude checking
|
||||
if(next_WP.options & MASK_OPTIONS_RELATIVE_ALT){
|
||||
// we desire a certain minimum altitude
|
||||
if (current_loc.alt > next_WP.alt){
|
||||
//if (current_loc.alt > next_WP.alt){
|
||||
if (current_loc.alt > target_altitude){
|
||||
|
||||
// we have reached that altitude
|
||||
wp_verify_byte |= NAV_ALTITUDE;
|
||||
}
|
||||
|
@ -509,7 +494,7 @@ static void do_change_alt()
|
|||
{
|
||||
Location temp = next_WP;
|
||||
condition_start = current_loc.alt;
|
||||
condition_value = command_cond_queue.alt;
|
||||
//condition_value = command_cond_queue.alt;
|
||||
temp.alt = command_cond_queue.alt;
|
||||
set_next_WP(&temp);
|
||||
}
|
||||
|
@ -582,9 +567,9 @@ static void do_yaw()
|
|||
static bool verify_wait_delay()
|
||||
{
|
||||
//Serial.print("vwd");
|
||||
if ((unsigned)(millis() - condition_start) > condition_value){
|
||||
if ((unsigned)(millis() - condition_start) > (unsigned)condition_value){
|
||||
//Serial.println("y");
|
||||
condition_value = 0;
|
||||
condition_value = 0;
|
||||
return true;
|
||||
}
|
||||
//Serial.println("n");
|
||||
|
@ -594,16 +579,14 @@ static bool verify_wait_delay()
|
|||
static bool verify_change_alt()
|
||||
{
|
||||
//Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt);
|
||||
if (condition_start < next_WP.alt){
|
||||
if ((int32_t)condition_start < next_WP.alt){
|
||||
// we are going higer
|
||||
if(current_loc.alt > next_WP.alt){
|
||||
condition_value = 0;
|
||||
return true;
|
||||
}
|
||||
}else{
|
||||
// we are going lower
|
||||
if(current_loc.alt < next_WP.alt){
|
||||
condition_value = 0;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -39,7 +39,7 @@ static void update_commands()
|
|||
//uint8_t tmp = g.command_index.get();
|
||||
//Serial.printf("command_index %u \n", tmp);
|
||||
|
||||
if (g.command_total <= 1 || g.command_index == 255)
|
||||
if (g.command_total <= 1 || g.command_index > 127)
|
||||
return;
|
||||
|
||||
if(command_nav_queue.id == NO_COMMAND){
|
||||
|
@ -62,8 +62,14 @@ static void update_commands()
|
|||
g.command_index = command_nav_index = 255;
|
||||
// if we are on the ground, enter stabilize, else Land
|
||||
if (land_complete == true){
|
||||
set_mode(STABILIZE);
|
||||
// disarm motors
|
||||
// So what state does this leave us in?
|
||||
// We are still in the same mode as what landed us,
|
||||
// so maybe we try to continue to descend just in case we are still in the air
|
||||
// This will also drive down the Iterm to -300
|
||||
set_new_altitude(-10000);
|
||||
|
||||
// We can't disarm the motors easily. We could very well be wrong
|
||||
//
|
||||
//init_disarm_motors();
|
||||
} else {
|
||||
set_mode(LAND);
|
||||
|
|
|
@ -1,5 +1,7 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
//
|
||||
#ifndef __ARDUCOPTER_CONFIG_H__
|
||||
#define __ARDUCOPTER_CONFIG_H__
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
|
@ -855,3 +857,7 @@
|
|||
#ifndef MAVLINK_TELEMETRY_PORT_DELAY
|
||||
# define MAVLINK_TELEMETRY_PORT_DELAY 2000
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#endif // __ARDUCOPTER_CONFIG_H__
|
||||
|
|
|
@ -0,0 +1,73 @@
|
|||
|
||||
#ifndef __ARDUCOPTER_CONFIG_MOTORS_H__
|
||||
#define __ARDUCOPTER_CONFIG_MOTORS_H__
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
|
||||
//
|
||||
// DO NOT EDIT this file to adjust your configuration. Create your own
|
||||
// APM_Config.h and use APM_Config.h.example as a reference.
|
||||
//
|
||||
// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING
|
||||
///
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
|
||||
#include "config.h"
|
||||
|
||||
//
|
||||
//
|
||||
// Output CH mapping for ArduCopter motor channels
|
||||
//
|
||||
//
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
# define MOT_1 CH_1
|
||||
# define MOT_2 CH_2
|
||||
# define MOT_3 CH_3
|
||||
# define MOT_4 CH_4
|
||||
# define MOT_5 CH_5
|
||||
# define MOT_6 CH_6
|
||||
# define MOT_7 CH_7
|
||||
# define MOT_8 CH_8
|
||||
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
|
||||
# define MOT_1 CH_1
|
||||
# define MOT_2 CH_2
|
||||
# define MOT_3 CH_3
|
||||
# define MOT_4 CH_4
|
||||
# define MOT_5 CH_7
|
||||
# define MOT_6 CH_8
|
||||
# define MOT_7 CH_10
|
||||
# define MOT_8 CH_11
|
||||
#endif
|
||||
|
||||
//
|
||||
//
|
||||
// Output CH mapping for TRI_FRAME yaw servo
|
||||
//
|
||||
//
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
/* TODO find out correct channel for APM2 TRI_YAW */
|
||||
# define CH_TRI_YAW (-1)
|
||||
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
|
||||
# define CH_TRI_YAW CH_7
|
||||
#endif
|
||||
|
||||
//
|
||||
//
|
||||
// Output CH mapping for Aux channels
|
||||
//
|
||||
//
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
/* Camera Pitch and Camera Roll: Not yet defined for APM2
|
||||
* They will likely be dependent on the frame config */
|
||||
# define CH_CAM_PITCH (-1)
|
||||
# define CH_CAM_ROLL (-1)
|
||||
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
|
||||
# define CH_CAM_PITCH CH_5
|
||||
# define CH_CAM_ROLL CH_6
|
||||
#endif
|
||||
|
||||
#endif // __ARDUCOPTER_CONFIG_MOTORS_H__
|
|
@ -106,6 +106,10 @@
|
|||
#define HIL_MODE_ATTITUDE 1
|
||||
#define HIL_MODE_SENSORS 2
|
||||
|
||||
#define ASCENDING 1
|
||||
#define DESCENDING -1
|
||||
#define REACHED_ALT 0
|
||||
|
||||
// Auto Pilot modes
|
||||
// ----------------
|
||||
#define STABILIZE 0 // hold level position
|
||||
|
|
|
@ -11,21 +11,22 @@ static void failsafe_on_event()
|
|||
{
|
||||
case AUTO:
|
||||
if (g.throttle_fs_action == 1) {
|
||||
// do_rtl sets the altitude to the current altitude by default
|
||||
set_mode(RTL);
|
||||
// send up up 10m
|
||||
next_WP.alt += 1000;
|
||||
// We add an additional 10m to the current altitude
|
||||
//next_WP.alt += 1000;
|
||||
set_new_altitude(target_altitude + 1000);
|
||||
}
|
||||
// 2 = Stay in AUTO and ignore failsafe
|
||||
|
||||
default:
|
||||
if(home_is_set == true){
|
||||
// home distance is in meters
|
||||
// This is to prevent accidental RTL
|
||||
if ((home_distance > 15) && (current_loc.alt > 400)){
|
||||
set_mode(RTL);
|
||||
// send up up 10m
|
||||
next_WP.alt += 1000;
|
||||
}
|
||||
// same as above ^
|
||||
// do_rtl sets the altitude to the current altitude by default
|
||||
set_mode(RTL);
|
||||
// We add an additional 10m to the current altitude
|
||||
//next_WP.alt += 1000;
|
||||
set_new_altitude(target_altitude + 1000);
|
||||
}else{
|
||||
// We have no GPS so we must land
|
||||
set_mode(LAND);
|
||||
|
|
|
@ -234,7 +234,7 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o
|
|||
static void init_motors_out()
|
||||
{
|
||||
#if INSTANT_PWM == 0
|
||||
APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4 );
|
||||
APM_RC.SetFastOutputChannels( _BV(CH_1) | _BV(CH_2) | _BV(CH_3) | _BV(CH_4) );
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
@ -142,3 +142,18 @@ set_servos_4()
|
|||
output_motors_disarmed();
|
||||
}
|
||||
}
|
||||
|
||||
int ch_of_mot( int mot ) {
|
||||
switch (mot) {
|
||||
case 1: return MOT_1;
|
||||
case 2: return MOT_2;
|
||||
case 3: return MOT_3;
|
||||
case 4: return MOT_4;
|
||||
case 5: return MOT_5;
|
||||
case 6: return MOT_6;
|
||||
case 7: return MOT_7;
|
||||
case 8: return MOT_8;
|
||||
}
|
||||
return (-1);
|
||||
}
|
||||
|
||||
|
|
|
@ -5,8 +5,8 @@
|
|||
static void init_motors_out()
|
||||
{
|
||||
#if INSTANT_PWM == 0
|
||||
APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4
|
||||
| MSK_CH_7 | MSK_CH_8 );
|
||||
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
|
||||
| _BV(MOT_5) | _BV(MOT_6) );
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -32,90 +32,88 @@ static void output_motors_armed()
|
|||
pitch_out = (float)g.rc_2.pwm_out * .866;
|
||||
|
||||
//left side
|
||||
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW Middle
|
||||
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW Front
|
||||
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back
|
||||
motor_out[MOT_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW Middle
|
||||
motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW Front
|
||||
motor_out[MOT_6] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back
|
||||
|
||||
//right side
|
||||
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW Middle
|
||||
motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW Front
|
||||
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW Back
|
||||
motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW Middle
|
||||
motor_out[MOT_5] = g.rc_3.radio_out - roll_out + pitch_out; // CCW Front
|
||||
motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW Back
|
||||
|
||||
}else{
|
||||
roll_out = (float)g.rc_1.pwm_out * .866;
|
||||
pitch_out = g.rc_2.pwm_out / 2;
|
||||
|
||||
//Front side
|
||||
motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT
|
||||
motor_out[CH_7] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT
|
||||
motor_out[CH_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT
|
||||
motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT
|
||||
motor_out[MOT_5] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT
|
||||
motor_out[MOT_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT
|
||||
|
||||
//Back side
|
||||
motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK
|
||||
motor_out[CH_3] = g.rc_3.radio_out + roll_out - pitch_out; // CW, BACK LEFT
|
||||
motor_out[CH_8] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT
|
||||
motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK
|
||||
motor_out[MOT_3] = g.rc_3.radio_out + roll_out - pitch_out; // CW, BACK LEFT
|
||||
motor_out[MOT_6] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT
|
||||
}
|
||||
|
||||
// Yaw
|
||||
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_4] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_2] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_5] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_4] += g.rc_4.pwm_out; // CCW
|
||||
|
||||
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_8] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_3] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_1] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_6] -= g.rc_4.pwm_out; // CW
|
||||
|
||||
|
||||
// Tridge's stability patch
|
||||
for (int i = CH_1; i<=CH_8; i++) {
|
||||
if(i == CH_5 || i == CH_6)
|
||||
continue;
|
||||
if (motor_out[i] > out_max) {
|
||||
// note that i^1 is the opposite motor
|
||||
motor_out[i^1] -= motor_out[i] - out_max;
|
||||
motor_out[i] = out_max;
|
||||
for (int m = 0; m <= 6; m++) {
|
||||
int c = ch_of_mot(m);
|
||||
int c_opp = ch_of_mot(m^1); // m^1 is the opposite motor. c_opp is channel of opposite motor.
|
||||
if (motor_out[c] > out_max) {
|
||||
motor_out[c_opp] -= motor_out[c] - out_max;
|
||||
motor_out[c] = out_max;
|
||||
}
|
||||
}
|
||||
|
||||
// limit output so motors don't stop
|
||||
motor_out[CH_1] = max(motor_out[CH_1], out_min);
|
||||
motor_out[CH_2] = max(motor_out[CH_2], out_min);
|
||||
motor_out[CH_3] = max(motor_out[CH_3], out_min);
|
||||
motor_out[CH_4] = max(motor_out[CH_4], out_min);
|
||||
motor_out[CH_7] = max(motor_out[CH_7], out_min);
|
||||
motor_out[CH_8] = max(motor_out[CH_8], out_min);
|
||||
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
|
||||
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
|
||||
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
|
||||
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
|
||||
motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
|
||||
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
|
||||
|
||||
#if CUT_MOTORS == ENABLED
|
||||
// if we are not sending a throttle output, we cut the motors
|
||||
if(g.rc_3.servo_out == 0){
|
||||
motor_out[CH_1] = g.rc_3.radio_min;
|
||||
motor_out[CH_2] = g.rc_3.radio_min;
|
||||
motor_out[CH_3] = g.rc_3.radio_min;
|
||||
motor_out[CH_4] = g.rc_3.radio_min;
|
||||
motor_out[CH_7] = g.rc_3.radio_min;
|
||||
motor_out[CH_8] = g.rc_3.radio_min;
|
||||
motor_out[MOT_1] = g.rc_3.radio_min;
|
||||
motor_out[MOT_2] = g.rc_3.radio_min;
|
||||
motor_out[MOT_3] = g.rc_3.radio_min;
|
||||
motor_out[MOT_4] = g.rc_3.radio_min;
|
||||
motor_out[MOT_5] = g.rc_3.radio_min;
|
||||
motor_out[MOT_6] = g.rc_3.radio_min;
|
||||
}
|
||||
#endif
|
||||
|
||||
// this filter slows the acceleration of motors vs the deceleration
|
||||
// Idea by Denny Rowland to help with his Yaw issue
|
||||
for(int8_t i = CH_1; i <= CH_8; i++ ) {
|
||||
if(i == CH_5 || i == CH_6)
|
||||
continue;
|
||||
if(motor_filtered[i] < motor_out[i]){
|
||||
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
|
||||
for(int8_t m = 0; m <= 6; m++ ) {
|
||||
int c = ch_of_mot(m);
|
||||
if(motor_filtered[c] < motor_out[c]){
|
||||
motor_filtered[c] = (motor_out[c] + motor_filtered[c]) / 2;
|
||||
}else{
|
||||
// don't filter
|
||||
motor_filtered[i] = motor_out[i];
|
||||
motor_filtered[c] = motor_out[c];
|
||||
}
|
||||
}
|
||||
|
||||
APM_RC.OutputCh(CH_1, motor_filtered[CH_1]);
|
||||
APM_RC.OutputCh(CH_2, motor_filtered[CH_2]);
|
||||
APM_RC.OutputCh(CH_3, motor_filtered[CH_3]);
|
||||
APM_RC.OutputCh(CH_4, motor_filtered[CH_4]);
|
||||
APM_RC.OutputCh(CH_7, motor_filtered[CH_7]);
|
||||
APM_RC.OutputCh(CH_8, motor_filtered[CH_8]);
|
||||
APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]);
|
||||
APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]);
|
||||
APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]);
|
||||
APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]);
|
||||
APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]);
|
||||
APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]);
|
||||
|
||||
#if INSTANT_PWM == 1
|
||||
// InstantPWM
|
||||
|
@ -140,43 +138,43 @@ static void output_motors_disarmed()
|
|||
}
|
||||
|
||||
// Send commands to motors
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
|
||||
}
|
||||
|
||||
static void output_motor_test()
|
||||
{
|
||||
motor_out[CH_1] = g.rc_3.radio_min;
|
||||
motor_out[CH_2] = g.rc_3.radio_min;
|
||||
motor_out[CH_3] = g.rc_3.radio_min;
|
||||
motor_out[CH_4] = g.rc_3.radio_min;
|
||||
motor_out[CH_7] = g.rc_3.radio_min;
|
||||
motor_out[CH_8] = g.rc_3.radio_min;
|
||||
motor_out[MOT_1] = g.rc_3.radio_min;
|
||||
motor_out[MOT_2] = g.rc_3.radio_min;
|
||||
motor_out[MOT_3] = g.rc_3.radio_min;
|
||||
motor_out[MOT_4] = g.rc_3.radio_min;
|
||||
motor_out[MOT_5] = g.rc_3.radio_min;
|
||||
motor_out[MOT_6] = g.rc_3.radio_min;
|
||||
|
||||
|
||||
if(g.frame_orientation == X_FRAME){
|
||||
// 31
|
||||
// 24
|
||||
if(g.rc_1.control_in > 3000){ // right
|
||||
motor_out[CH_1] += 100;
|
||||
motor_out[MOT_1] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_1.control_in < -3000){ // left
|
||||
motor_out[CH_2] += 100;
|
||||
motor_out[MOT_2] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_2.control_in > 3000){ // back
|
||||
motor_out[CH_8] += 100;
|
||||
motor_out[CH_4] += 100;
|
||||
motor_out[MOT_6] += 100;
|
||||
motor_out[MOT_4] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_2.control_in < -3000){ // front
|
||||
motor_out[CH_7] += 100;
|
||||
motor_out[CH_3] += 100;
|
||||
motor_out[MOT_5] += 100;
|
||||
motor_out[MOT_3] += 100;
|
||||
}
|
||||
|
||||
}else{
|
||||
|
@ -184,56 +182,56 @@ static void output_motor_test()
|
|||
// 2 1
|
||||
// 4
|
||||
if(g.rc_1.control_in > 3000){ // right
|
||||
motor_out[CH_4] += 100;
|
||||
motor_out[CH_8] += 100;
|
||||
motor_out[MOT_4] += 100;
|
||||
motor_out[MOT_6] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_1.control_in < -3000){ // left
|
||||
motor_out[CH_7] += 100;
|
||||
motor_out[CH_3] += 100;
|
||||
motor_out[MOT_5] += 100;
|
||||
motor_out[MOT_3] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_2.control_in > 3000){ // back
|
||||
motor_out[CH_2] += 100;
|
||||
motor_out[MOT_2] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_2.control_in < -3000){ // front
|
||||
motor_out[CH_1] += 100;
|
||||
motor_out[MOT_1] += 100;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
||||
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
|
||||
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
|
||||
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
|
||||
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
|
||||
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
|
||||
APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
|
||||
APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
|
||||
APM_RC.OutputCh(MOT_3, motor_out[MOT_3]);
|
||||
APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
|
||||
APM_RC.OutputCh(MOT_5, motor_out[MOT_5]);
|
||||
APM_RC.OutputCh(MOT_6, motor_out[MOT_6]);
|
||||
}
|
||||
|
||||
/*
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
}
|
||||
*/
|
||||
|
|
|
@ -5,8 +5,8 @@
|
|||
static void init_motors_out()
|
||||
{
|
||||
#if INSTANT_PWM == 0
|
||||
APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4
|
||||
| MSK_CH_7 | MSK_CH_8 | MSK_CH_10 | MSK_CH_11 );
|
||||
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
|
||||
| _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8) );
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -32,40 +32,40 @@ static void output_motors_armed()
|
|||
pitch_out = (float)g.rc_2.pwm_out * 0.4;
|
||||
|
||||
//Front side
|
||||
motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT
|
||||
motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT
|
||||
motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT
|
||||
motor_out[MOT_5] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT
|
||||
|
||||
//Back side
|
||||
motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT
|
||||
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT
|
||||
motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT
|
||||
motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT
|
||||
|
||||
//Left side
|
||||
motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW LEFT FRONT
|
||||
motor_out[CH_8] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out; // CCW LEFT BACK
|
||||
motor_out[MOT_7] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW LEFT FRONT
|
||||
motor_out[MOT_6] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out; // CCW LEFT BACK
|
||||
|
||||
//Right side
|
||||
motor_out[CH_11] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW RIGHT BACK
|
||||
motor_out[CH_3] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT
|
||||
motor_out[MOT_8] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW RIGHT BACK
|
||||
motor_out[MOT_3] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT
|
||||
|
||||
}else if(g.frame_orientation == PLUS_FRAME){
|
||||
roll_out = (float)g.rc_1.pwm_out * 0.71;
|
||||
pitch_out = (float)g.rc_2.pwm_out * 0.71;
|
||||
|
||||
//Front side
|
||||
motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT
|
||||
motor_out[CH_3] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT
|
||||
motor_out[CH_7] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT
|
||||
motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT
|
||||
motor_out[MOT_3] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT
|
||||
motor_out[MOT_5] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT
|
||||
|
||||
//Left side
|
||||
motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT
|
||||
motor_out[MOT_7] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT
|
||||
|
||||
//Right side
|
||||
motor_out[CH_11] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT
|
||||
motor_out[MOT_8] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT
|
||||
|
||||
//Back side
|
||||
motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK
|
||||
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT
|
||||
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT
|
||||
motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK
|
||||
motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT
|
||||
motor_out[MOT_6] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT
|
||||
|
||||
}else if(g.frame_orientation == V_FRAME){
|
||||
|
||||
|
@ -83,92 +83,91 @@ static void output_motors_armed()
|
|||
pitch_out4 = (float)g.rc_2.pwm_out * 0.98;
|
||||
|
||||
//Front side
|
||||
motor_out[CH_10] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT
|
||||
motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT
|
||||
motor_out[MOT_7] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT
|
||||
motor_out[MOT_5] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT
|
||||
|
||||
//Left side
|
||||
motor_out[CH_1] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT
|
||||
motor_out[CH_3] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK
|
||||
motor_out[MOT_1] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT
|
||||
motor_out[MOT_3] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK
|
||||
|
||||
//Right side
|
||||
motor_out[CH_2] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK
|
||||
motor_out[CH_8] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT
|
||||
motor_out[MOT_2] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK
|
||||
motor_out[MOT_6] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT
|
||||
|
||||
//Back side
|
||||
motor_out[CH_11] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT
|
||||
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT
|
||||
motor_out[MOT_8] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT
|
||||
motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT
|
||||
|
||||
}
|
||||
|
||||
// Yaw
|
||||
motor_out[CH_3] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_4] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_8] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_3] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_4] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_5] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_6] += g.rc_4.pwm_out; // CCW
|
||||
|
||||
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_2] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_10] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_11] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_1] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_2] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_7] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_8] -= g.rc_4.pwm_out; // CW
|
||||
|
||||
|
||||
// TODO add stability patch
|
||||
motor_out[CH_1] = min(motor_out[CH_1], out_max);
|
||||
motor_out[CH_2] = min(motor_out[CH_2], out_max);
|
||||
motor_out[CH_3] = min(motor_out[CH_3], out_max);
|
||||
motor_out[CH_4] = min(motor_out[CH_4], out_max);
|
||||
motor_out[CH_7] = min(motor_out[CH_7], out_max);
|
||||
motor_out[CH_8] = min(motor_out[CH_8], out_max);
|
||||
motor_out[CH_10] = min(motor_out[CH_10], out_max);
|
||||
motor_out[CH_11] = min(motor_out[CH_11], out_max);
|
||||
motor_out[MOT_1] = min(motor_out[MOT_1], out_max);
|
||||
motor_out[MOT_2] = min(motor_out[MOT_2], out_max);
|
||||
motor_out[MOT_3] = min(motor_out[MOT_3], out_max);
|
||||
motor_out[MOT_4] = min(motor_out[MOT_4], out_max);
|
||||
motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
|
||||
motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
|
||||
motor_out[MOT_7] = min(motor_out[MOT_7], out_max);
|
||||
motor_out[MOT_8] = min(motor_out[MOT_8], out_max);
|
||||
|
||||
|
||||
// limit output so motors don't stop
|
||||
motor_out[CH_1] = max(motor_out[CH_1], out_min);
|
||||
motor_out[CH_2] = max(motor_out[CH_2], out_min);
|
||||
motor_out[CH_3] = max(motor_out[CH_3], out_min);
|
||||
motor_out[CH_4] = max(motor_out[CH_4], out_min);
|
||||
motor_out[CH_7] = max(motor_out[CH_7], out_min);
|
||||
motor_out[CH_8] = max(motor_out[CH_8], out_min);
|
||||
motor_out[CH_10] = max(motor_out[CH_10], out_min);
|
||||
motor_out[CH_11] = max(motor_out[CH_11], out_min);
|
||||
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
|
||||
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
|
||||
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
|
||||
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
|
||||
motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
|
||||
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
|
||||
motor_out[MOT_7] = max(motor_out[MOT_7], out_min);
|
||||
motor_out[MOT_8] = max(motor_out[MOT_8], out_min);
|
||||
|
||||
|
||||
#if CUT_MOTORS == ENABLED
|
||||
// if we are not sending a throttle output, we cut the motors
|
||||
if(g.rc_3.servo_out == 0){
|
||||
motor_out[CH_1] = g.rc_3.radio_min;
|
||||
motor_out[CH_2] = g.rc_3.radio_min;
|
||||
motor_out[CH_3] = g.rc_3.radio_min;
|
||||
motor_out[CH_4] = g.rc_3.radio_min;
|
||||
motor_out[CH_7] = g.rc_3.radio_min;
|
||||
motor_out[CH_8] = g.rc_3.radio_min;
|
||||
motor_out[CH_10] = g.rc_3.radio_min;
|
||||
motor_out[CH_11] = g.rc_3.radio_min;
|
||||
motor_out[MOT_1] = g.rc_3.radio_min;
|
||||
motor_out[MOT_2] = g.rc_3.radio_min;
|
||||
motor_out[MOT_3] = g.rc_3.radio_min;
|
||||
motor_out[MOT_4] = g.rc_3.radio_min;
|
||||
motor_out[MOT_5] = g.rc_3.radio_min;
|
||||
motor_out[MOT_6] = g.rc_3.radio_min;
|
||||
motor_out[MOT_7] = g.rc_3.radio_min;
|
||||
motor_out[MOT_8] = g.rc_3.radio_min;
|
||||
}
|
||||
#endif
|
||||
|
||||
// this filter slows the acceleration of motors vs the deceleration
|
||||
// Idea by Denny Rowland to help with his Yaw issue
|
||||
for(int8_t i = CH_1; i <= CH_11; i++ ) {
|
||||
if(i == CH_5 || i == CH_6 || i == CH_9)
|
||||
continue;
|
||||
if(motor_filtered[i] < motor_out[i]){
|
||||
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
|
||||
for(int8_t m = 0; m <= 8; m++ ) {
|
||||
int c = ch_of_mot(m);
|
||||
if(motor_filtered[c] < motor_out[c]){
|
||||
motor_filtered[c] = (motor_out[c] + motor_filtered[c]) / 2;
|
||||
}else{
|
||||
// don't filter
|
||||
motor_filtered[i] = motor_out[i];
|
||||
motor_filtered[c] = motor_out[c];
|
||||
}
|
||||
}
|
||||
|
||||
APM_RC.OutputCh(CH_1, motor_filtered[CH_1]);
|
||||
APM_RC.OutputCh(CH_2, motor_filtered[CH_2]);
|
||||
APM_RC.OutputCh(CH_3, motor_filtered[CH_3]);
|
||||
APM_RC.OutputCh(CH_4, motor_filtered[CH_4]);
|
||||
APM_RC.OutputCh(CH_7, motor_filtered[CH_7]);
|
||||
APM_RC.OutputCh(CH_8, motor_filtered[CH_8]);
|
||||
APM_RC.OutputCh(CH_10, motor_filtered[CH_10]);
|
||||
APM_RC.OutputCh(CH_11, motor_filtered[CH_11]);
|
||||
APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]);
|
||||
APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]);
|
||||
APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]);
|
||||
APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]);
|
||||
APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]);
|
||||
APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]);
|
||||
APM_RC.OutputCh(MOT_7, motor_filtered[MOT_7]);
|
||||
APM_RC.OutputCh(MOT_8, motor_filtered[MOT_8]);
|
||||
|
||||
#if INSTANT_PWM == 1
|
||||
// InstantPWM
|
||||
|
@ -192,86 +191,86 @@ static void output_motors_disarmed()
|
|||
}
|
||||
|
||||
// Send commands to motors
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
|
||||
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
|
||||
}
|
||||
|
||||
static void output_motor_test()
|
||||
{
|
||||
if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME )
|
||||
{
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
if( g.frame_orientation == V_FRAME )
|
||||
{
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
}
|
||||
}
|
||||
|
|
|
@ -5,8 +5,8 @@
|
|||
static void init_motors_out()
|
||||
{
|
||||
#if INSTANT_PWM == 0
|
||||
APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4
|
||||
| MSK_CH_7 | MSK_CH_8 | MSK_CH_10 | MSK_CH_11 );
|
||||
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
|
||||
| _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8) );
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -32,20 +32,20 @@ static void output_motors_armed()
|
|||
pitch_out = (float)g.rc_2.pwm_out * .707;
|
||||
|
||||
// Front Left
|
||||
motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP
|
||||
motor_out[CH_8] = g.rc_3.radio_out + roll_out + pitch_out; // CW
|
||||
motor_out[MOT_5] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP
|
||||
motor_out[MOT_6] = g.rc_3.radio_out + roll_out + pitch_out; // CW
|
||||
|
||||
// Front Right
|
||||
motor_out[CH_10] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP
|
||||
motor_out[CH_11] = g.rc_3.radio_out - roll_out + pitch_out; // CW
|
||||
motor_out[MOT_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP
|
||||
motor_out[MOT_8] = g.rc_3.radio_out - roll_out + pitch_out; // CW
|
||||
|
||||
// Back Left
|
||||
motor_out[CH_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // CCW TOP
|
||||
motor_out[CH_4] = g.rc_3.radio_out + roll_out - pitch_out; // CW
|
||||
motor_out[MOT_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // CCW TOP
|
||||
motor_out[MOT_4] = g.rc_3.radio_out + roll_out - pitch_out; // CW
|
||||
|
||||
// Back Right
|
||||
motor_out[CH_1] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // CCW TOP
|
||||
motor_out[CH_2] = g.rc_3.radio_out - roll_out - pitch_out; // CW
|
||||
motor_out[MOT_1] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // CCW TOP
|
||||
motor_out[MOT_2] = g.rc_3.radio_out - roll_out - pitch_out; // CW
|
||||
|
||||
|
||||
|
||||
|
@ -54,73 +54,72 @@ static void output_motors_armed()
|
|||
pitch_out = g.rc_2.pwm_out;
|
||||
|
||||
// Left
|
||||
motor_out[CH_7] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; // CCW TOP
|
||||
motor_out[CH_8] = g.rc_3.radio_out - roll_out; // CW
|
||||
motor_out[MOT_5] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; // CCW TOP
|
||||
motor_out[MOT_6] = g.rc_3.radio_out - roll_out; // CW
|
||||
|
||||
// Right
|
||||
motor_out[CH_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; // CCW TOP
|
||||
motor_out[CH_2] = g.rc_3.radio_out + roll_out; // CW
|
||||
motor_out[MOT_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; // CCW TOP
|
||||
motor_out[MOT_2] = g.rc_3.radio_out + roll_out; // CW
|
||||
|
||||
// Front
|
||||
motor_out[CH_10] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; // CCW TOP
|
||||
motor_out[CH_11] = g.rc_3.radio_out + pitch_out; // CW
|
||||
motor_out[MOT_7] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; // CCW TOP
|
||||
motor_out[MOT_8] = g.rc_3.radio_out + pitch_out; // CW
|
||||
|
||||
// Back
|
||||
motor_out[CH_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; // CCW TOP
|
||||
motor_out[CH_4] = g.rc_3.radio_out - pitch_out; // CW
|
||||
motor_out[MOT_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; // CCW TOP
|
||||
motor_out[MOT_4] = g.rc_3.radio_out - pitch_out; // CW
|
||||
|
||||
}
|
||||
|
||||
// Yaw
|
||||
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_3] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_10] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_1] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_3] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_5] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_7] += g.rc_4.pwm_out; // CCW
|
||||
|
||||
motor_out[CH_2] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_8] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_11] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_2] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_6] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_8] -= g.rc_4.pwm_out; // CW
|
||||
|
||||
// TODO add stability patch
|
||||
motor_out[CH_1] = min(motor_out[CH_1], out_max);
|
||||
motor_out[CH_2] = min(motor_out[CH_2], out_max);
|
||||
motor_out[CH_3] = min(motor_out[CH_3], out_max);
|
||||
motor_out[CH_4] = min(motor_out[CH_4], out_max);
|
||||
motor_out[CH_7] = min(motor_out[CH_7], out_max);
|
||||
motor_out[CH_8] = min(motor_out[CH_8], out_max);
|
||||
motor_out[CH_10] = min(motor_out[CH_10], out_max);
|
||||
motor_out[CH_11] = min(motor_out[CH_11], out_max);
|
||||
motor_out[MOT_1] = min(motor_out[MOT_1], out_max);
|
||||
motor_out[MOT_2] = min(motor_out[MOT_2], out_max);
|
||||
motor_out[MOT_3] = min(motor_out[MOT_3], out_max);
|
||||
motor_out[MOT_4] = min(motor_out[MOT_4], out_max);
|
||||
motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
|
||||
motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
|
||||
motor_out[MOT_7] = min(motor_out[MOT_7], out_max);
|
||||
motor_out[MOT_8] = min(motor_out[MOT_8], out_max);
|
||||
|
||||
// limit output so motors don't stop
|
||||
motor_out[CH_1] = max(motor_out[CH_1], out_min);
|
||||
motor_out[CH_2] = max(motor_out[CH_2], out_min);
|
||||
motor_out[CH_3] = max(motor_out[CH_3], out_min);
|
||||
motor_out[CH_4] = max(motor_out[CH_4], out_min);
|
||||
motor_out[CH_7] = max(motor_out[CH_7], out_min);
|
||||
motor_out[CH_8] = max(motor_out[CH_8], out_min);
|
||||
motor_out[CH_10] = max(motor_out[CH_10], out_min);
|
||||
motor_out[CH_11] = max(motor_out[CH_11], out_min);
|
||||
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
|
||||
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
|
||||
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
|
||||
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
|
||||
motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
|
||||
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
|
||||
motor_out[MOT_7] = max(motor_out[MOT_7], out_min);
|
||||
motor_out[MOT_8] = max(motor_out[MOT_8], out_min);
|
||||
|
||||
#if CUT_MOTORS == ENABLED
|
||||
// if we are not sending a throttle output, we cut the motors
|
||||
if(g.rc_3.servo_out == 0){
|
||||
motor_out[CH_1] = g.rc_3.radio_min;
|
||||
motor_out[CH_2] = g.rc_3.radio_min;
|
||||
motor_out[CH_3] = g.rc_3.radio_min;
|
||||
motor_out[CH_4] = g.rc_3.radio_min;
|
||||
motor_out[CH_7] = g.rc_3.radio_min;
|
||||
motor_out[CH_8] = g.rc_3.radio_min;
|
||||
motor_out[CH_10] = g.rc_3.radio_min;
|
||||
motor_out[CH_11] = g.rc_3.radio_min;
|
||||
motor_out[MOT_1] = g.rc_3.radio_min;
|
||||
motor_out[MOT_2] = g.rc_3.radio_min;
|
||||
motor_out[MOT_3] = g.rc_3.radio_min;
|
||||
motor_out[MOT_4] = g.rc_3.radio_min;
|
||||
motor_out[MOT_5] = g.rc_3.radio_min;
|
||||
motor_out[MOT_6] = g.rc_3.radio_min;
|
||||
motor_out[MOT_7] = g.rc_3.radio_min;
|
||||
motor_out[MOT_8] = g.rc_3.radio_min;
|
||||
}
|
||||
#endif
|
||||
|
||||
// this filter slows the acceleration of motors vs the deceleration
|
||||
// Idea by Denny Rowland to help with his Yaw issue
|
||||
for(int8_t i = CH_1; i <= CH_11; i++ ) {
|
||||
if(i == CH_5 || i == CH_6 || i == CH_9)
|
||||
continue;
|
||||
for(int8_t m = 0; m <= 8; m++ ) {
|
||||
int i = ch_of_mot(m);
|
||||
if(motor_filtered[i] < motor_out[i]){
|
||||
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
|
||||
}else{
|
||||
|
@ -129,14 +128,14 @@ static void output_motors_armed()
|
|||
}
|
||||
}
|
||||
|
||||
APM_RC.OutputCh(CH_1, motor_filtered[CH_1]);
|
||||
APM_RC.OutputCh(CH_2, motor_filtered[CH_2]);
|
||||
APM_RC.OutputCh(CH_3, motor_filtered[CH_3]);
|
||||
APM_RC.OutputCh(CH_4, motor_filtered[CH_4]);
|
||||
APM_RC.OutputCh(CH_7, motor_filtered[CH_7]);
|
||||
APM_RC.OutputCh(CH_8, motor_filtered[CH_8]);
|
||||
APM_RC.OutputCh(CH_10, motor_filtered[CH_10]);
|
||||
APM_RC.OutputCh(CH_11, motor_filtered[CH_11]);
|
||||
APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]);
|
||||
APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]);
|
||||
APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]);
|
||||
APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]);
|
||||
APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]);
|
||||
APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]);
|
||||
APM_RC.OutputCh(MOT_7, motor_filtered[MOT_7]);
|
||||
APM_RC.OutputCh(MOT_8, motor_filtered[MOT_8]);
|
||||
|
||||
#if INSTANT_PWM == 1
|
||||
// InstantPWM
|
||||
|
@ -160,48 +159,48 @@ static void output_motors_disarmed()
|
|||
}
|
||||
|
||||
// Send commands to motors
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
|
||||
}
|
||||
|
||||
static void output_motor_test()
|
||||
{
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
|
|
|
@ -5,7 +5,7 @@
|
|||
static void init_motors_out()
|
||||
{
|
||||
#if INSTANT_PWM == 0
|
||||
APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4 );
|
||||
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4) );
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -33,12 +33,12 @@ static void output_motors_armed()
|
|||
pitch_out = g.rc_2.pwm_out * .707;
|
||||
|
||||
// left
|
||||
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT
|
||||
motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK
|
||||
motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT
|
||||
motor_out[MOT_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK
|
||||
|
||||
// right
|
||||
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT
|
||||
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // BACK
|
||||
motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT
|
||||
motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // BACK
|
||||
|
||||
}else{
|
||||
|
||||
|
@ -46,20 +46,20 @@ static void output_motors_armed()
|
|||
pitch_out = g.rc_2.pwm_out;
|
||||
|
||||
// right motor
|
||||
motor_out[CH_1] = g.rc_3.radio_out - roll_out;
|
||||
motor_out[MOT_1] = g.rc_3.radio_out - roll_out;
|
||||
// left motor
|
||||
motor_out[CH_2] = g.rc_3.radio_out + roll_out;
|
||||
motor_out[MOT_2] = g.rc_3.radio_out + roll_out;
|
||||
// front motor
|
||||
motor_out[CH_3] = g.rc_3.radio_out + pitch_out;
|
||||
motor_out[MOT_3] = g.rc_3.radio_out + pitch_out;
|
||||
// back motor
|
||||
motor_out[CH_4] = g.rc_3.radio_out - pitch_out;
|
||||
motor_out[MOT_4] = g.rc_3.radio_out - pitch_out;
|
||||
}
|
||||
|
||||
// Yaw input
|
||||
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_1] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_2] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_3] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
|
||||
|
||||
/* We need to clip motor output at out_max. When cipping a motors
|
||||
* output we also need to compensate for the instability by
|
||||
|
@ -67,7 +67,7 @@ static void output_motors_armed()
|
|||
* ensures that we retain control when one or more of the motors
|
||||
* is at its maximum output
|
||||
*/
|
||||
for (int i=CH_1; i<=CH_4; i++) {
|
||||
for (int i=MOT_1; i<=MOT_4; i++) {
|
||||
if (motor_out[i] > out_max) {
|
||||
// note that i^1 is the opposite motor
|
||||
motor_out[i^1] -= motor_out[i] - out_max;
|
||||
|
@ -76,24 +76,24 @@ static void output_motors_armed()
|
|||
}
|
||||
|
||||
// limit output so motors don't stop
|
||||
motor_out[CH_1] = max(motor_out[CH_1], out_min);
|
||||
motor_out[CH_2] = max(motor_out[CH_2], out_min);
|
||||
motor_out[CH_3] = max(motor_out[CH_3], out_min);
|
||||
motor_out[CH_4] = max(motor_out[CH_4], out_min);
|
||||
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
|
||||
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
|
||||
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
|
||||
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
|
||||
|
||||
#if CUT_MOTORS == ENABLED
|
||||
// if we are not sending a throttle output, we cut the motors
|
||||
if(g.rc_3.servo_out == 0){
|
||||
motor_out[CH_1] = g.rc_3.radio_min;
|
||||
motor_out[CH_2] = g.rc_3.radio_min;
|
||||
motor_out[CH_3] = g.rc_3.radio_min;
|
||||
motor_out[CH_4] = g.rc_3.radio_min;
|
||||
motor_out[MOT_1] = g.rc_3.radio_min;
|
||||
motor_out[MOT_2] = g.rc_3.radio_min;
|
||||
motor_out[MOT_3] = g.rc_3.radio_min;
|
||||
motor_out[MOT_4] = g.rc_3.radio_min;
|
||||
}
|
||||
#endif
|
||||
|
||||
// this filter slows the acceleration of motors vs the deceleration
|
||||
// Idea by Denny Rowland to help with his Yaw issue
|
||||
for(int8_t i=CH_1; i <= CH_4; i++ ) {
|
||||
for(int8_t i=MOT_1; i <= MOT_4; i++ ) {
|
||||
if(motor_filtered[i] < motor_out[i]){
|
||||
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
|
||||
}else{
|
||||
|
@ -102,10 +102,10 @@ static void output_motors_armed()
|
|||
}
|
||||
}
|
||||
|
||||
APM_RC.OutputCh(CH_1, motor_filtered[CH_1]);
|
||||
APM_RC.OutputCh(CH_2, motor_filtered[CH_2]);
|
||||
APM_RC.OutputCh(CH_3, motor_filtered[CH_3]);
|
||||
APM_RC.OutputCh(CH_4, motor_filtered[CH_4]);
|
||||
APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]);
|
||||
APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]);
|
||||
APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]);
|
||||
APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]);
|
||||
|
||||
#if INSTANT_PWM == 1
|
||||
// InstantPWM
|
||||
|
@ -130,52 +130,52 @@ static void output_motors_disarmed()
|
|||
}
|
||||
|
||||
// Send commands to motors
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
}
|
||||
|
||||
/*
|
||||
//static void debug_motors()
|
||||
{
|
||||
Serial.printf("1:%d\t2:%d\t3:%d\t4:%d\n",
|
||||
motor_out[CH_1],
|
||||
motor_out[CH_2],
|
||||
motor_out[CH_3],
|
||||
motor_out[CH_4]);
|
||||
motor_out[MOT_1],
|
||||
motor_out[MOT_2],
|
||||
motor_out[MOT_3],
|
||||
motor_out[MOT_4]);
|
||||
}
|
||||
//*/
|
||||
|
||||
static void output_motor_test()
|
||||
{
|
||||
motor_out[CH_1] = g.rc_3.radio_min;
|
||||
motor_out[CH_2] = g.rc_3.radio_min;
|
||||
motor_out[CH_3] = g.rc_3.radio_min;
|
||||
motor_out[CH_4] = g.rc_3.radio_min;
|
||||
motor_out[MOT_1] = g.rc_3.radio_min;
|
||||
motor_out[MOT_2] = g.rc_3.radio_min;
|
||||
motor_out[MOT_3] = g.rc_3.radio_min;
|
||||
motor_out[MOT_4] = g.rc_3.radio_min;
|
||||
|
||||
|
||||
if(g.frame_orientation == X_FRAME){
|
||||
// 31
|
||||
// 24
|
||||
if(g.rc_1.control_in > 3000){
|
||||
motor_out[CH_1] += 100;
|
||||
motor_out[CH_4] += 100;
|
||||
motor_out[MOT_1] += 100;
|
||||
motor_out[MOT_4] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_1.control_in < -3000){
|
||||
motor_out[CH_2] += 100;
|
||||
motor_out[CH_3] += 100;
|
||||
motor_out[MOT_2] += 100;
|
||||
motor_out[MOT_3] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_2.control_in > 3000){
|
||||
motor_out[CH_2] += 100;
|
||||
motor_out[CH_4] += 100;
|
||||
motor_out[MOT_2] += 100;
|
||||
motor_out[MOT_4] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_2.control_in < -3000){
|
||||
motor_out[CH_1] += 100;
|
||||
motor_out[CH_3] += 100;
|
||||
motor_out[MOT_1] += 100;
|
||||
motor_out[MOT_3] += 100;
|
||||
}
|
||||
|
||||
}else{
|
||||
|
@ -183,22 +183,22 @@ static void output_motor_test()
|
|||
// 2 1
|
||||
// 4
|
||||
if(g.rc_1.control_in > 3000)
|
||||
motor_out[CH_1] += 100;
|
||||
motor_out[MOT_1] += 100;
|
||||
|
||||
if(g.rc_1.control_in < -3000)
|
||||
motor_out[CH_2] += 100;
|
||||
motor_out[MOT_2] += 100;
|
||||
|
||||
if(g.rc_2.control_in > 3000)
|
||||
motor_out[CH_4] += 100;
|
||||
motor_out[MOT_4] += 100;
|
||||
|
||||
if(g.rc_2.control_in < -3000)
|
||||
motor_out[CH_3] += 100;
|
||||
motor_out[MOT_3] += 100;
|
||||
}
|
||||
|
||||
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
||||
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
|
||||
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
|
||||
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
|
||||
APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
|
||||
APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
|
||||
APM_RC.OutputCh(MOT_3, motor_out[MOT_3]);
|
||||
APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -4,7 +4,7 @@
|
|||
static void init_motors_out()
|
||||
{
|
||||
#if INSTANT_PWM == 0
|
||||
APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_4 );
|
||||
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_4) );
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -28,50 +28,50 @@ static void output_motors_armed()
|
|||
int pitch_out = g.rc_2.pwm_out / 2;
|
||||
|
||||
//left front
|
||||
motor_out[CH_2] = g.rc_3.radio_out + roll_out + pitch_out;
|
||||
motor_out[MOT_2] = g.rc_3.radio_out + roll_out + pitch_out;
|
||||
//right front
|
||||
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out;
|
||||
motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out;
|
||||
// rear
|
||||
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
|
||||
motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
|
||||
|
||||
//motor_out[CH_4] += (float)(abs(g.rc_4.control_in)) * .013;
|
||||
//motor_out[MOT_4] += (float)(abs(g.rc_4.control_in)) * .013;
|
||||
|
||||
// Tridge's stability patch
|
||||
if (motor_out[CH_1] > out_max) {
|
||||
motor_out[CH_2] -= (motor_out[CH_1] - out_max) >> 1;
|
||||
motor_out[CH_4] -= (motor_out[CH_1] - out_max) >> 1;
|
||||
motor_out[CH_1] = out_max;
|
||||
if (motor_out[MOT_1] > out_max) {
|
||||
motor_out[MOT_2] -= (motor_out[MOT_1] - out_max) >> 1;
|
||||
motor_out[MOT_4] -= (motor_out[MOT_1] - out_max) >> 1;
|
||||
motor_out[MOT_1] = out_max;
|
||||
}
|
||||
|
||||
if (motor_out[CH_2] > out_max) {
|
||||
motor_out[CH_1] -= (motor_out[CH_2] - out_max) >> 1;
|
||||
motor_out[CH_4] -= (motor_out[CH_2] - out_max) >> 1;
|
||||
motor_out[CH_2] = out_max;
|
||||
if (motor_out[MOT_2] > out_max) {
|
||||
motor_out[MOT_1] -= (motor_out[MOT_2] - out_max) >> 1;
|
||||
motor_out[MOT_4] -= (motor_out[MOT_2] - out_max) >> 1;
|
||||
motor_out[MOT_2] = out_max;
|
||||
}
|
||||
|
||||
if (motor_out[CH_4] > out_max) {
|
||||
motor_out[CH_1] -= (motor_out[CH_4] - out_max) >> 1;
|
||||
motor_out[CH_2] -= (motor_out[CH_4] - out_max) >> 1;
|
||||
motor_out[CH_4] = out_max;
|
||||
if (motor_out[MOT_4] > out_max) {
|
||||
motor_out[MOT_1] -= (motor_out[MOT_4] - out_max) >> 1;
|
||||
motor_out[MOT_2] -= (motor_out[MOT_4] - out_max) >> 1;
|
||||
motor_out[MOT_4] = out_max;
|
||||
}
|
||||
|
||||
// limit output so motors don't stop
|
||||
motor_out[CH_1] = max(motor_out[CH_1], out_min);
|
||||
motor_out[CH_2] = max(motor_out[CH_2], out_min);
|
||||
motor_out[CH_4] = max(motor_out[CH_4], out_min);
|
||||
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
|
||||
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
|
||||
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
|
||||
|
||||
#if CUT_MOTORS == ENABLED
|
||||
// if we are not sending a throttle output, we cut the motors
|
||||
if(g.rc_3.servo_out == 0){
|
||||
motor_out[CH_1] = g.rc_3.radio_min;
|
||||
motor_out[CH_2] = g.rc_3.radio_min;
|
||||
motor_out[CH_4] = g.rc_3.radio_min;
|
||||
motor_out[MOT_1] = g.rc_3.radio_min;
|
||||
motor_out[MOT_2] = g.rc_3.radio_min;
|
||||
motor_out[MOT_4] = g.rc_3.radio_min;
|
||||
}
|
||||
#endif
|
||||
|
||||
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
||||
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
|
||||
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
|
||||
APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
|
||||
APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
|
||||
APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
|
||||
|
||||
#if INSTANT_PWM == 1
|
||||
// InstantPWM
|
||||
|
@ -94,33 +94,33 @@ static void output_motors_disarmed()
|
|||
}
|
||||
|
||||
// Send commands to motors
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
}
|
||||
|
||||
static void output_motor_test()
|
||||
{
|
||||
motor_out[CH_1] = g.rc_3.radio_min;
|
||||
motor_out[CH_2] = g.rc_3.radio_min;
|
||||
motor_out[CH_4] = g.rc_3.radio_min;
|
||||
motor_out[MOT_1] = g.rc_3.radio_min;
|
||||
motor_out[MOT_2] = g.rc_3.radio_min;
|
||||
motor_out[MOT_4] = g.rc_3.radio_min;
|
||||
|
||||
|
||||
if(g.rc_1.control_in > 3000){ // right
|
||||
motor_out[CH_1] += 100;
|
||||
motor_out[MOT_1] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_1.control_in < -3000){ // left
|
||||
motor_out[CH_2] += 100;
|
||||
motor_out[MOT_2] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_2.control_in > 3000){ // back
|
||||
motor_out[CH_4] += 100;
|
||||
motor_out[MOT_4] += 100;
|
||||
}
|
||||
|
||||
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
||||
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
|
||||
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
|
||||
APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
|
||||
APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
|
||||
APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -7,8 +7,8 @@
|
|||
static void init_motors_out()
|
||||
{
|
||||
#if INSTANT_PWM == 0
|
||||
APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4
|
||||
| MSK_CH_7 | MSK_CH_8 );
|
||||
APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4)
|
||||
| _BV(MOT_5) | _BV(MOT_6) );
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -30,24 +30,24 @@ static void output_motors_armed()
|
|||
|
||||
// Multi-Wii Mix
|
||||
//left
|
||||
motor_out[CH_2] = (g.rc_3.radio_out * g.top_bottom_ratio) + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT TOP - CW
|
||||
motor_out[CH_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW
|
||||
motor_out[MOT_2] = (g.rc_3.radio_out * g.top_bottom_ratio) + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT TOP - CW
|
||||
motor_out[MOT_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW
|
||||
//right
|
||||
motor_out[CH_7] = (g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW
|
||||
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW
|
||||
motor_out[MOT_5] = (g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW
|
||||
motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW
|
||||
//back
|
||||
motor_out[CH_8] = (g.rc_3.radio_out * g.top_bottom_ratio) - (g.rc_2.pwm_out * 4 / 3); // REAR TOP - CCW
|
||||
motor_out[CH_4] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // BOTTOM_REAR - CW
|
||||
motor_out[MOT_6] = (g.rc_3.radio_out * g.top_bottom_ratio) - (g.rc_2.pwm_out * 4 / 3); // REAR TOP - CCW
|
||||
motor_out[MOT_4] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // BOTTOM_REAR - CW
|
||||
|
||||
//left
|
||||
motor_out[CH_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW
|
||||
motor_out[CH_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW
|
||||
motor_out[MOT_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW
|
||||
motor_out[MOT_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW
|
||||
//right
|
||||
motor_out[CH_7] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW
|
||||
motor_out[CH_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW
|
||||
motor_out[MOT_5] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW
|
||||
motor_out[MOT_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW
|
||||
//back
|
||||
motor_out[CH_8] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW
|
||||
motor_out[CH_4] -= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW
|
||||
motor_out[MOT_6] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW
|
||||
motor_out[MOT_4] -= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW
|
||||
|
||||
|
||||
/*
|
||||
|
@ -55,62 +55,61 @@ static void output_motors_armed()
|
|||
int pitch_out = g.rc_2.pwm_out / 2;
|
||||
|
||||
//left
|
||||
motor_out[CH_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP
|
||||
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
|
||||
motor_out[MOT_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP
|
||||
motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
|
||||
|
||||
//right
|
||||
motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP
|
||||
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
|
||||
motor_out[MOT_5] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP
|
||||
motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
|
||||
|
||||
//back
|
||||
motor_out[CH_8] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP
|
||||
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
|
||||
motor_out[MOT_6] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP
|
||||
motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
|
||||
|
||||
// Yaw
|
||||
//top
|
||||
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_7] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[CH_8] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_2] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_5] += g.rc_4.pwm_out; // CCW
|
||||
motor_out[MOT_6] += g.rc_4.pwm_out; // CCW
|
||||
|
||||
//bottom
|
||||
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[CH_4] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_3] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_1] -= g.rc_4.pwm_out; // CW
|
||||
motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
|
||||
*/
|
||||
|
||||
// TODO: add stability patch
|
||||
motor_out[CH_1] = min(motor_out[CH_1], out_max);
|
||||
motor_out[CH_2] = min(motor_out[CH_2], out_max);
|
||||
motor_out[CH_3] = min(motor_out[CH_3], out_max);
|
||||
motor_out[CH_4] = min(motor_out[CH_4], out_max);
|
||||
motor_out[CH_7] = min(motor_out[CH_7], out_max);
|
||||
motor_out[CH_8] = min(motor_out[CH_8], out_max);
|
||||
motor_out[MOT_1] = min(motor_out[MOT_1], out_max);
|
||||
motor_out[MOT_2] = min(motor_out[MOT_2], out_max);
|
||||
motor_out[MOT_3] = min(motor_out[MOT_3], out_max);
|
||||
motor_out[MOT_4] = min(motor_out[MOT_4], out_max);
|
||||
motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
|
||||
motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
|
||||
|
||||
// limit output so motors don't stop
|
||||
motor_out[CH_1] = max(motor_out[CH_1], out_min);
|
||||
motor_out[CH_2] = max(motor_out[CH_2], out_min);
|
||||
motor_out[CH_3] = max(motor_out[CH_3], out_min);
|
||||
motor_out[CH_4] = max(motor_out[CH_4], out_min);
|
||||
motor_out[CH_7] = max(motor_out[CH_7], out_min);
|
||||
motor_out[CH_8] = max(motor_out[CH_8], out_min);
|
||||
motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
|
||||
motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
|
||||
motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
|
||||
motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
|
||||
motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
|
||||
motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
|
||||
|
||||
#if CUT_MOTORS == ENABLED
|
||||
// if we are not sending a throttle output, we cut the motors
|
||||
if(g.rc_3.servo_out == 0){
|
||||
motor_out[CH_1] = g.rc_3.radio_min;
|
||||
motor_out[CH_2] = g.rc_3.radio_min;
|
||||
motor_out[CH_3] = g.rc_3.radio_min;
|
||||
motor_out[CH_4] = g.rc_3.radio_min;
|
||||
motor_out[CH_7] = g.rc_3.radio_min;
|
||||
motor_out[CH_8] = g.rc_3.radio_min;
|
||||
motor_out[MOT_1] = g.rc_3.radio_min;
|
||||
motor_out[MOT_2] = g.rc_3.radio_min;
|
||||
motor_out[MOT_3] = g.rc_3.radio_min;
|
||||
motor_out[MOT_4] = g.rc_3.radio_min;
|
||||
motor_out[MOT_5] = g.rc_3.radio_min;
|
||||
motor_out[MOT_6] = g.rc_3.radio_min;
|
||||
}
|
||||
#endif
|
||||
|
||||
// this filter slows the acceleration of motors vs the deceleration
|
||||
// Idea by Denny Rowland to help with his Yaw issue
|
||||
for(int8_t i = CH_1; i <= CH_8; i++ ) {
|
||||
if(i == CH_5 || i == CH_6)
|
||||
continue;
|
||||
for(int8_t m = 0; m <= 6; m++ ) {
|
||||
int i = ch_of_mot(m);
|
||||
if(motor_filtered[i] < motor_out[i]){
|
||||
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
|
||||
}else{
|
||||
|
@ -119,12 +118,12 @@ static void output_motors_armed()
|
|||
}
|
||||
}
|
||||
|
||||
APM_RC.OutputCh(CH_1, motor_filtered[CH_1]);
|
||||
APM_RC.OutputCh(CH_2, motor_filtered[CH_2]);
|
||||
APM_RC.OutputCh(CH_3, motor_filtered[CH_3]);
|
||||
APM_RC.OutputCh(CH_4, motor_filtered[CH_4]);
|
||||
APM_RC.OutputCh(CH_7, motor_filtered[CH_7]);
|
||||
APM_RC.OutputCh(CH_8, motor_filtered[CH_8]);
|
||||
APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]);
|
||||
APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]);
|
||||
APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]);
|
||||
APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]);
|
||||
APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]);
|
||||
APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]);
|
||||
|
||||
#if INSTANT_PWM == 1
|
||||
// InstantPWM
|
||||
|
@ -148,45 +147,45 @@ static void output_motors_disarmed()
|
|||
}
|
||||
|
||||
// Send commands to motors
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
|
||||
}
|
||||
|
||||
static void output_motor_test()
|
||||
{
|
||||
motor_out[CH_1] = g.rc_3.radio_min;
|
||||
motor_out[CH_2] = g.rc_3.radio_min;
|
||||
motor_out[CH_3] = g.rc_3.radio_min;
|
||||
motor_out[CH_4] = g.rc_3.radio_min;
|
||||
motor_out[CH_7] = g.rc_3.radio_min;
|
||||
motor_out[CH_8] = g.rc_3.radio_min;
|
||||
motor_out[MOT_1] = g.rc_3.radio_min;
|
||||
motor_out[MOT_2] = g.rc_3.radio_min;
|
||||
motor_out[MOT_3] = g.rc_3.radio_min;
|
||||
motor_out[MOT_4] = g.rc_3.radio_min;
|
||||
motor_out[MOT_5] = g.rc_3.radio_min;
|
||||
motor_out[MOT_6] = g.rc_3.radio_min;
|
||||
|
||||
|
||||
if(g.rc_1.control_in > 3000){ // right
|
||||
motor_out[CH_1] += 100;
|
||||
motor_out[CH_7] += 100;
|
||||
motor_out[MOT_1] += 100;
|
||||
motor_out[MOT_5] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_1.control_in < -3000){ // left
|
||||
motor_out[CH_2] += 100;
|
||||
motor_out[CH_3] += 100;
|
||||
motor_out[MOT_2] += 100;
|
||||
motor_out[MOT_3] += 100;
|
||||
}
|
||||
|
||||
if(g.rc_2.control_in > 3000){ // back
|
||||
motor_out[CH_8] += 100;
|
||||
motor_out[CH_4] += 100;
|
||||
motor_out[MOT_6] += 100;
|
||||
motor_out[MOT_4] += 100;
|
||||
}
|
||||
|
||||
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
|
||||
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
|
||||
APM_RC.OutputCh(CH_3, motor_out[CH_4]);
|
||||
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
|
||||
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
|
||||
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
|
||||
APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
|
||||
APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
|
||||
APM_RC.OutputCh(MOT_3, motor_out[MOT_4]);
|
||||
APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
|
||||
APM_RC.OutputCh(MOT_5, motor_out[MOT_5]);
|
||||
APM_RC.OutputCh(MOT_6, motor_out[MOT_6]);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -367,12 +367,125 @@ static void update_crosstrack(void)
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
static int32_t get_altitude_error()
|
||||
{
|
||||
// Next_WP alt is our target alt
|
||||
// It changes based on climb rate
|
||||
// until it reaches the target_altitude
|
||||
return next_WP.alt - current_loc.alt;
|
||||
}
|
||||
|
||||
static void clear_new_altitude()
|
||||
{
|
||||
alt_change_flag = REACHED_ALT;
|
||||
}
|
||||
|
||||
static void set_new_altitude(int32_t _new_alt)
|
||||
{
|
||||
// just to be clear
|
||||
next_WP.alt = current_loc.alt;
|
||||
|
||||
// for calculating the delta time
|
||||
alt_change_timer = millis();
|
||||
|
||||
// save the target altitude
|
||||
target_altitude = _new_alt;
|
||||
|
||||
// reset our altitude integrator
|
||||
alt_change = 0;
|
||||
|
||||
// save the original altitude
|
||||
original_altitude = current_loc.alt;
|
||||
|
||||
// to decide if we have reached the target altitude
|
||||
if(target_altitude > original_altitude){
|
||||
// we are below, going up
|
||||
alt_change_flag = ASCENDING;
|
||||
Serial.printf("go up\n");
|
||||
}else if(target_altitude < original_altitude){
|
||||
// we are above, going down
|
||||
alt_change_flag = DESCENDING;
|
||||
Serial.printf("go down\n");
|
||||
}else{
|
||||
// No Change
|
||||
alt_change_flag = REACHED_ALT;
|
||||
Serial.printf("reached alt\n");
|
||||
}
|
||||
|
||||
//Serial.printf("new alt: %d Org alt: %d\n", target_altitude, original_altitude);
|
||||
}
|
||||
|
||||
static int32_t get_new_altitude()
|
||||
{
|
||||
// returns a new next_WP.alt
|
||||
|
||||
if(alt_change_flag == ASCENDING){
|
||||
// we are below, going up
|
||||
if(current_loc.alt >= target_altitude){
|
||||
alt_change_flag = REACHED_ALT;
|
||||
}
|
||||
|
||||
// we shouldn't command past our target
|
||||
if(next_WP.alt >= target_altitude){
|
||||
return target_altitude;
|
||||
}
|
||||
}else if (alt_change_flag == DESCENDING){
|
||||
// we are above, going down
|
||||
if(current_loc.alt <= target_altitude)
|
||||
alt_change_flag = REACHED_ALT;
|
||||
|
||||
// we shouldn't command past our target
|
||||
if(next_WP.alt <= target_altitude){
|
||||
return target_altitude;
|
||||
}
|
||||
}
|
||||
|
||||
// if we have reached our target altitude, return the target alt
|
||||
if(alt_change_flag == REACHED_ALT){
|
||||
return target_altitude;
|
||||
}
|
||||
|
||||
int32_t diff = abs(next_WP.alt - target_altitude);
|
||||
int8_t _scale = 4;
|
||||
|
||||
if (next_WP.alt < target_altitude){
|
||||
// we are below the target alt
|
||||
|
||||
if(diff < 200){
|
||||
// we are going up
|
||||
_scale = 5;
|
||||
} else {
|
||||
_scale = 4;
|
||||
}
|
||||
|
||||
}else {
|
||||
// we are above the target
|
||||
// stay at 16 for speed
|
||||
//_scale = 16;
|
||||
|
||||
if(diff < 400){
|
||||
// we are going down
|
||||
_scale = 5;
|
||||
|
||||
}else if(diff < 100){
|
||||
_scale = 6;
|
||||
}
|
||||
}
|
||||
|
||||
int32_t change = (millis() - alt_change_timer) >> _scale;
|
||||
|
||||
if(alt_change_flag == ASCENDING){
|
||||
alt_change += change;
|
||||
}else{
|
||||
alt_change -= change;
|
||||
}
|
||||
// for generating delta time
|
||||
alt_change_timer = millis();
|
||||
|
||||
return original_altitude + alt_change;
|
||||
}
|
||||
|
||||
|
||||
static int32_t wrap_360(int32_t error)
|
||||
{
|
||||
if (error > 36000) error -= 36000;
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
//Function that will read the radio data, limit servos and trigger a failsafe
|
||||
// ----------------------------------------------------------------------------
|
||||
static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
|
||||
static int8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
|
||||
|
||||
static void default_dead_zones()
|
||||
{
|
||||
|
@ -55,8 +55,8 @@ static void init_rc_out()
|
|||
init_motors_out();
|
||||
|
||||
// this is the camera pitch5 and roll6
|
||||
APM_RC.OutputCh(CH_5, 1500);
|
||||
APM_RC.OutputCh(CH_6, 1500);
|
||||
APM_RC.OutputCh(CH_CAM_PITCH, 1500);
|
||||
APM_RC.OutputCh(CH_CAM_ROLL, 1500);
|
||||
|
||||
for(byte i = 0; i < 5; i++){
|
||||
delay(20);
|
||||
|
@ -103,18 +103,18 @@ void output_min()
|
|||
#if FRAME_CONFIG == HELI_FRAME
|
||||
heli_move_servos_to_mid();
|
||||
#else
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); // Initialization of servo outputs
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
|
||||
#endif
|
||||
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
|
||||
|
||||
#if FRAME_CONFIG == OCTA_FRAME
|
||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
|
||||
#endif
|
||||
|
||||
}
|
||||
|
@ -153,12 +153,16 @@ static void throttle_failsafe(uint16_t pwm)
|
|||
// throttle has dropped below the mark
|
||||
failsafeCounter++;
|
||||
if (failsafeCounter == 9){
|
||||
SendDebug("MSG FS ON ");
|
||||
SendDebugln(pwm, DEC);
|
||||
//
|
||||
}else if(failsafeCounter == 10) {
|
||||
// Don't enter Failsafe if we are not armed
|
||||
if(motor_armed == true)
|
||||
// home distance is in meters
|
||||
// This is to prevent accidental RTL
|
||||
if((motor_armed == true) && (home_distance > 10) && (current_loc.alt > 400)){
|
||||
SendDebug("MSG FS ON ");
|
||||
SendDebugln(pwm, DEC);
|
||||
set_failsafe(true);
|
||||
}
|
||||
}else if (failsafeCounter > 10){
|
||||
failsafeCounter = 11;
|
||||
}
|
||||
|
|
|
@ -271,7 +271,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
|||
{
|
||||
byte _switchPosition = 0;
|
||||
byte _oldSwitchPosition = 0;
|
||||
byte mode = 0;
|
||||
int8_t mode = 0;
|
||||
|
||||
Serial.printf_P(PSTR("\nMode switch to edit, aileron: select modes, rudder: Simple on/off\n"));
|
||||
print_hit_enter();
|
||||
|
@ -1102,16 +1102,16 @@ init_esc()
|
|||
read_radio();
|
||||
delay(100);
|
||||
dancing_light();
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_5, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_6, g.rc_3.radio_in);
|
||||
|
||||
#if FRAME_CONFIG == OCTA_FRAME
|
||||
APM_RC.OutputCh(CH_10, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_11, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_7, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_8, g.rc_3.radio_in);
|
||||
#endif
|
||||
|
||||
}
|
||||
|
|
|
@ -321,7 +321,7 @@ static void init_ardupilot()
|
|||
#endif
|
||||
|
||||
// initialise sonar
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE && CONFIG_SONAR == ENABLED
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
init_sonar();
|
||||
#endif
|
||||
|
||||
|
@ -490,9 +490,7 @@ static void set_mode(byte mode)
|
|||
roll_pitch_mode = ALT_HOLD_RP;
|
||||
throttle_mode = ALT_HOLD_THR;
|
||||
|
||||
next_WP = current_loc;
|
||||
// 1m is the alt hold limit
|
||||
next_WP.alt = max(next_WP.alt, 100);
|
||||
set_next_WP(¤t_loc);
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
|
@ -508,9 +506,7 @@ static void set_mode(byte mode)
|
|||
yaw_mode = CIRCLE_YAW;
|
||||
roll_pitch_mode = CIRCLE_RP;
|
||||
throttle_mode = CIRCLE_THR;
|
||||
next_WP = current_loc;
|
||||
|
||||
// reset the desired circle angle
|
||||
set_next_WP(¤t_loc);
|
||||
circle_angle = 0;
|
||||
break;
|
||||
|
||||
|
@ -518,14 +514,14 @@ static void set_mode(byte mode)
|
|||
yaw_mode = LOITER_YAW;
|
||||
roll_pitch_mode = LOITER_RP;
|
||||
throttle_mode = LOITER_THR;
|
||||
next_WP = current_loc;
|
||||
set_next_WP(¤t_loc);
|
||||
break;
|
||||
|
||||
case POSITION:
|
||||
yaw_mode = YAW_HOLD;
|
||||
roll_pitch_mode = ROLL_PITCH_AUTO;
|
||||
throttle_mode = THROTTLE_MANUAL;
|
||||
next_WP = current_loc;
|
||||
set_next_WP(¤t_loc);
|
||||
break;
|
||||
|
||||
case GUIDED:
|
||||
|
@ -579,11 +575,12 @@ static void set_mode(byte mode)
|
|||
// removes the navigation from roll and pitch commands, but leaves the wind compensation
|
||||
reset_nav();
|
||||
|
||||
// removes the navigation from roll and pitch commands, but leaves the wind compensation
|
||||
if(GPS_enabled)
|
||||
#if WIND_COMP_STAB == 1
|
||||
if(GPS_enabled){
|
||||
wp_control = NO_NAV_MODE;
|
||||
update_nav_wp();
|
||||
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
Log_Write_Mode(control_mode);
|
||||
|
|
|
@ -167,7 +167,7 @@ test_eedump(uint8_t argc, const Menu::arg *argv)
|
|||
g.rc_4.control_in,
|
||||
g.rc_4.radio_out);
|
||||
|
||||
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
|
||||
APM_RC.OutputCh(CH_TRI_YAW, g.rc_4.radio_out);
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
|
@ -765,10 +765,10 @@ test_current(uint8_t argc, const Menu::arg *argv)
|
|||
current_amps,
|
||||
current_total);
|
||||
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_4, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_1, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_2, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_3, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(MOT_4, g.rc_3.radio_in);
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
|
|
|
@ -168,7 +168,7 @@ def fly_RTL(mavproxy, mav, side=60):
|
|||
print("# Enter RTL")
|
||||
mavproxy.send('switch 3\n')
|
||||
tstart = time.time()
|
||||
while time.time() < tstart + 120:
|
||||
while time.time() < tstart + 200:
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
pos = current_location(mav)
|
||||
#delta = get_distance(start, pos)
|
||||
|
@ -208,7 +208,7 @@ def fly_simple(mavproxy, mav, side=60, timeout=120):
|
|||
'''fly Simple, flying N then E'''
|
||||
mavproxy.send('switch 6\n')
|
||||
wait_mode(mav, 'STABILIZE')
|
||||
mavproxy.send('rc 3 1450\n')
|
||||
mavproxy.send('rc 3 1440\n')
|
||||
tstart = time.time()
|
||||
failed = False
|
||||
|
||||
|
@ -246,7 +246,7 @@ def land(mavproxy, mav, timeout=60):
|
|||
mavproxy.send('switch 2\n')
|
||||
wait_mode(mav, 'LAND')
|
||||
print("Entered Landing Mode")
|
||||
ret = wait_altitude(mav, -5, 5)
|
||||
ret = wait_altitude(mav, -5, 1)
|
||||
print("LANDING: ok= %s" % ret)
|
||||
return ret
|
||||
|
||||
|
@ -437,8 +437,8 @@ def fly_ArduCopter(viewerip=None):
|
|||
failed = True
|
||||
|
||||
# Loiter for 30 seconds
|
||||
print("# Loiter for 30 seconds")
|
||||
if not loiter(mavproxy, mav, 30):
|
||||
print("# Loiter for 45 seconds")
|
||||
if not loiter(mavproxy, mav, 45):
|
||||
print("loiter failed")
|
||||
failed = True
|
||||
|
||||
|
|
|
@ -195,7 +195,9 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
|
|||
seq = m.seq
|
||||
m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
|
||||
wp_dist = m.wp_dist
|
||||
print("test: WP %u (wp_dist=%u), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, current_wp, wpnum_end))
|
||||
m = mav.recv_match(type='VFR_HUD', blocking=True)
|
||||
|
||||
print("test: WP %u (wp_dist=%u Alt=%d), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, m.alt, current_wp, wpnum_end))
|
||||
if seq == current_wp+1 or (seq > current_wp+1 and allow_skip):
|
||||
print("test: Starting new waypoint %u" % seq)
|
||||
tstart = time.time()
|
||||
|
|
|
@ -168,10 +168,10 @@ while True:
|
|||
frame_count += 1
|
||||
t = time.time()
|
||||
if t - lastt > 1.0:
|
||||
print("%.2f fps zspeed=%.2f zaccel=%.2f h=%.1f a=%.1f yaw=%.1f yawrate=%.1f" % (
|
||||
frame_count/(t-lastt),
|
||||
a.velocity.z, a.accel.z, a.position.z, a.altitude,
|
||||
a.yaw, a.yaw_rate))
|
||||
#print("%.2f fps zspeed=%.2f zaccel=%.2f h=%.1f a=%.1f yaw=%.1f yawrate=%.1f" % (
|
||||
# frame_count/(t-lastt),
|
||||
# a.velocity.z, a.accel.z, a.position.z, a.altitude,
|
||||
# a.yaw, a.yaw_rate))
|
||||
lastt = t
|
||||
frame_count = 0
|
||||
frame_end = time.time()
|
||||
|
|
|
@ -21,20 +21,6 @@
|
|||
#define CH_10 9
|
||||
#define CH_11 10
|
||||
|
||||
#define MSK_CH_1 (1 << CH_1)
|
||||
#define MSK_CH_2 (1 << CH_2)
|
||||
#define MSK_CH_3 (1 << CH_3)
|
||||
#define MSK_CH_4 (1 << CH_4)
|
||||
#define MSK_CH_5 (1 << CH_5)
|
||||
#define MSK_CH_6 (1 << CH_6)
|
||||
#define MSK_CH_7 (1 << CH_7)
|
||||
#define MSK_CH_8 (1 << CH_8)
|
||||
#define MSK_CH_9 (1 << CH_9)
|
||||
#define MSK_CH_10 (1 << CH_10)
|
||||
#define MSK_CH_11 (1 << CH_11)
|
||||
|
||||
|
||||
|
||||
|
||||
#define NUM_CHANNELS 8
|
||||
|
||||
|
|
|
@ -216,16 +216,16 @@ void APM_RC_APM1::Force_Out6_Out7(void)
|
|||
|
||||
void APM_RC_APM1::SetFastOutputChannels(uint32_t chmask)
|
||||
{
|
||||
if ((chmask & ( MSK_CH_1 | MSK_CH_2 | MSK_CH_9)) != 0)
|
||||
if ((chmask & ( _BV(CH_1) | _BV(CH_2) | _BV(CH_9))) != 0)
|
||||
_set_speed_ch1_ch2_ch9(OUTPUT_SPEED_400HZ);
|
||||
|
||||
if ((chmask & ( MSK_CH_3 | MSK_CH_4 | MSK_CH_10 )) != 0)
|
||||
if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_10))) != 0)
|
||||
_set_speed_ch3_ch4_ch10(OUTPUT_SPEED_400HZ);
|
||||
|
||||
if ((chmask & ( MSK_CH_5 | MSK_CH_6 )) != 0)
|
||||
if ((chmask & ( _BV(CH_5) | _BV(CH_6))) != 0)
|
||||
_set_speed_ch5_ch6(OUTPUT_SPEED_400HZ);
|
||||
|
||||
if ((chmask & ( MSK_CH_7 | MSK_CH_8 | MSK_CH_11 )) != 0)
|
||||
if ((chmask & ( _BV(CH_7) | _BV(CH_8) | _BV(CH_11))) != 0)
|
||||
_set_speed_ch7_ch8_ch11(OUTPUT_SPEED_400HZ);
|
||||
|
||||
}
|
||||
|
|
|
@ -197,13 +197,13 @@ void APM_RC_APM2::Force_Out6_Out7(void) { }
|
|||
|
||||
void APM_RC_APM2::SetFastOutputChannels(uint32_t chmask)
|
||||
{
|
||||
if ((chmask & ( MSK_CH_1 | MSK_CH_2 )) != 0)
|
||||
if ((chmask & ( _BV(CH_1) | _BV(CH_2))) != 0)
|
||||
_set_speed_ch1_ch2(OUTPUT_SPEED_400HZ);
|
||||
|
||||
if ((chmask & ( MSK_CH_3 | MSK_CH_4 | MSK_CH_5 )) != 0)
|
||||
if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0)
|
||||
_set_speed_ch3_ch4_ch5(OUTPUT_SPEED_400HZ);
|
||||
|
||||
if ((chmask & ( MSK_CH_6 | MSK_CH_7 | MSK_CH_8 )) != 0)
|
||||
if ((chmask & ( _BV(CH_6) | _BV(CH_7) | _BV(CH_8))) != 0)
|
||||
_set_speed_ch6_ch7_ch8(OUTPUT_SPEED_400HZ);
|
||||
}
|
||||
|
||||
|
|
|
@ -79,7 +79,7 @@ class AP_ADC_HIL : public AP_ADC
|
|||
// @param index the axis for the accelerometer(0-x,1-y,2-z)
|
||||
inline void setAccel(uint8_t index, int16_t val) {
|
||||
int16_t temp = val * accelScale[index] / 1000 + accelBias[index];
|
||||
adcValue[sensors[index+3]] = (sensors[index+3] < 0) ? -temp : temp;
|
||||
adcValue[sensors[index+3]] = (sensorSign[index+3] < 0) ? -temp : temp;
|
||||
}
|
||||
|
||||
///
|
||||
|
|
|
@ -30,7 +30,8 @@ Menu::Menu(const prog_char *prompt, const Menu::command *commands, uint8_t entri
|
|||
void
|
||||
Menu::run(void)
|
||||
{
|
||||
uint8_t len, i, ret;
|
||||
int8_t ret;
|
||||
uint8_t len, i;
|
||||
uint8_t argc;
|
||||
int c;
|
||||
char *s;
|
||||
|
@ -123,12 +124,12 @@ Menu::run(void)
|
|||
return;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (cmd_found==false)
|
||||
{
|
||||
Serial.println("Invalid command, type 'help'");
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -1,65 +1,65 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
|
||||
/*
|
||||
AP_RangeFinder_MaxsonarXL.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
|
||||
infrared proximity sensor
|
||||
Code by Jose Julio and Randy Mackay. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
Sparkfun URL: http://www.sparkfun.com/products/9491
|
||||
datasheet: http://www.sparkfun.com/datasheets/Sensors/Proximity/XL-EZ0-Datasheet.pdf
|
||||
|
||||
Sensor should be connected to one of the analog ports
|
||||
|
||||
Variables:
|
||||
int raw_value : raw value from the sensor
|
||||
int distance : distance in cm
|
||||
int max_distance : maximum measurable distance (in cm)
|
||||
int min_distance : minimum measurable distance (in cm)
|
||||
|
||||
Methods:
|
||||
read() : read value from analog port and returns the distance (in cm)
|
||||
|
||||
*/
|
||||
|
||||
// AVR LibC Includes
|
||||
#include "WConstants.h"
|
||||
#include "AP_RangeFinder_MaxsonarXL.h"
|
||||
|
||||
// Constructor //////////////////////////////////////////////////////////////
|
||||
|
||||
AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source,
|
||||
ModeFilter *filter) :
|
||||
RangeFinder(source, filter), _scaler(AP_RANGEFINDER_MAXSONARXL_SCALER)
|
||||
{
|
||||
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
|
||||
min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage)
|
||||
{
|
||||
float type_scaler = 1.0;
|
||||
switch(sonar_type) {
|
||||
case AP_RANGEFINDER_MAXSONARXL:
|
||||
type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER;
|
||||
min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
|
||||
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
|
||||
break;
|
||||
case AP_RANGEFINDER_MAXSONARLV:
|
||||
type_scaler = AP_RANGEFINDER_MAXSONARLV_SCALER;
|
||||
min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE;
|
||||
max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE;
|
||||
break;
|
||||
case AP_RANGEFINDER_MAXSONARXLL:
|
||||
type_scaler = AP_RANGEFINDER_MAXSONARXLL_SCALER;
|
||||
min_distance = AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE;
|
||||
max_distance = AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE;
|
||||
break;
|
||||
}
|
||||
_scaler = type_scaler * adc_refence_voltage / 5.0;
|
||||
return _scaler;
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
|
||||
/*
|
||||
AP_RangeFinder_MaxsonarXL.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
|
||||
infrared proximity sensor
|
||||
Code by Jose Julio and Randy Mackay. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
Sparkfun URL: http://www.sparkfun.com/products/9491
|
||||
datasheet: http://www.sparkfun.com/datasheets/Sensors/Proximity/XL-EZ0-Datasheet.pdf
|
||||
|
||||
Sensor should be connected to one of the analog ports
|
||||
|
||||
Variables:
|
||||
int raw_value : raw value from the sensor
|
||||
int distance : distance in cm
|
||||
int max_distance : maximum measurable distance (in cm)
|
||||
int min_distance : minimum measurable distance (in cm)
|
||||
|
||||
Methods:
|
||||
read() : read value from analog port and returns the distance (in cm)
|
||||
|
||||
*/
|
||||
|
||||
// AVR LibC Includes
|
||||
#include "WConstants.h"
|
||||
#include "AP_RangeFinder_MaxsonarXL.h"
|
||||
|
||||
// Constructor //////////////////////////////////////////////////////////////
|
||||
|
||||
AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter):
|
||||
RangeFinder(source, filter),
|
||||
_scaler(AP_RANGEFINDER_MAXSONARXL_SCALER)
|
||||
{
|
||||
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
|
||||
min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage)
|
||||
{
|
||||
float type_scaler = 1.0;
|
||||
switch(sonar_type) {
|
||||
case AP_RANGEFINDER_MAXSONARXL:
|
||||
type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER;
|
||||
min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
|
||||
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
|
||||
break;
|
||||
case AP_RANGEFINDER_MAXSONARLV:
|
||||
type_scaler = AP_RANGEFINDER_MAXSONARLV_SCALER;
|
||||
min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE;
|
||||
max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE;
|
||||
break;
|
||||
case AP_RANGEFINDER_MAXSONARXLL:
|
||||
type_scaler = AP_RANGEFINDER_MAXSONARXLL_SCALER;
|
||||
min_distance = AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE;
|
||||
max_distance = AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE;
|
||||
break;
|
||||
}
|
||||
_scaler = type_scaler * adc_refence_voltage / 5.0;
|
||||
return _scaler;
|
||||
}
|
|
@ -1,33 +1,34 @@
|
|||
#ifndef AP_RangeFinder_MaxsonarXL_H
|
||||
#define AP_RangeFinder_MaxsonarXL_H
|
||||
|
||||
#include "RangeFinder.h"
|
||||
|
||||
// XL-EZ0 (aka XL)
|
||||
#define AP_RANGEFINDER_MAXSONARXL 0
|
||||
#define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0
|
||||
#define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20
|
||||
#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765
|
||||
|
||||
// LV-EZ0 (aka LV)
|
||||
#define AP_RANGEFINDER_MAXSONARLV 1
|
||||
#define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0)
|
||||
#define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15
|
||||
#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645
|
||||
|
||||
// XL-EZL0 (aka XLL)
|
||||
#define AP_RANGEFINDER_MAXSONARXLL 2
|
||||
#define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0
|
||||
#define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20
|
||||
#define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068
|
||||
|
||||
class AP_RangeFinder_MaxsonarXL : public RangeFinder
|
||||
{
|
||||
public:
|
||||
AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter);
|
||||
int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; } // read value from analog port and return distance in cm
|
||||
float calculate_scaler(int sonar_type, float adc_refence_voltage);
|
||||
private:
|
||||
float _scaler; // used to account for different sonar types
|
||||
};
|
||||
#endif
|
||||
#ifndef AP_RangeFinder_MaxsonarXL_H
|
||||
#define AP_RangeFinder_MaxsonarXL_H
|
||||
|
||||
#include "RangeFinder.h"
|
||||
|
||||
// XL-EZ0 (aka XL)
|
||||
#define AP_RANGEFINDER_MAXSONARXL 0
|
||||
#define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0
|
||||
#define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20
|
||||
#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765
|
||||
|
||||
// LV-EZ0 (aka LV)
|
||||
#define AP_RANGEFINDER_MAXSONARLV 1
|
||||
#define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0)
|
||||
#define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15
|
||||
#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645
|
||||
|
||||
// XL-EZL0 (aka XLL)
|
||||
#define AP_RANGEFINDER_MAXSONARXLL 2
|
||||
#define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0
|
||||
#define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20
|
||||
#define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068
|
||||
|
||||
class AP_RangeFinder_MaxsonarXL : public RangeFinder
|
||||
{
|
||||
public:
|
||||
AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter);
|
||||
int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; } // read value from analog port and return distance in cm
|
||||
float calculate_scaler(int sonar_type, float adc_refence_voltage);
|
||||
|
||||
private:
|
||||
float _scaler; // used to account for different sonar types
|
||||
};
|
||||
#endif
|
||||
|
|
|
@ -1,42 +1,41 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
|
||||
/*
|
||||
AP_RangeFinder_SharpGP2Y.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
|
||||
infrared proximity sensor
|
||||
Code by Jose Julio and Randy Mackay. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
Sensor should be conected to one of the analog ports
|
||||
|
||||
Sparkfun URL: http://www.sparkfun.com/products/8958
|
||||
datasheet: http://www.sparkfun.com/datasheets/Sensors/Infrared/gp2y0a02yk_e.pdf
|
||||
|
||||
Variables:
|
||||
int raw_value : raw value from the sensor
|
||||
int distance : distance in cm
|
||||
int max_distance : maximum measurable distance (in cm)
|
||||
int min_distance : minimum measurable distance (in cm)
|
||||
|
||||
Methods:
|
||||
read() : read value from analog port
|
||||
|
||||
*/
|
||||
|
||||
// AVR LibC Includes
|
||||
#include "WConstants.h"
|
||||
#include "AP_RangeFinder_SharpGP2Y.h"
|
||||
|
||||
// Constructor //////////////////////////////////////////////////////////////
|
||||
|
||||
AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source,
|
||||
ModeFilter *filter) :
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
|
||||
/*
|
||||
AP_RangeFinder_SharpGP2Y.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
|
||||
infrared proximity sensor
|
||||
Code by Jose Julio and Randy Mackay. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
Sensor should be conected to one of the analog ports
|
||||
|
||||
Sparkfun URL: http://www.sparkfun.com/products/8958
|
||||
datasheet: http://www.sparkfun.com/datasheets/Sensors/Infrared/gp2y0a02yk_e.pdf
|
||||
|
||||
Variables:
|
||||
int raw_value : raw value from the sensor
|
||||
int distance : distance in cm
|
||||
int max_distance : maximum measurable distance (in cm)
|
||||
int min_distance : minimum measurable distance (in cm)
|
||||
|
||||
Methods:
|
||||
read() : read value from analog port
|
||||
|
||||
*/
|
||||
|
||||
// AVR LibC Includes
|
||||
#include "WConstants.h"
|
||||
#include "AP_RangeFinder_SharpGP2Y.h"
|
||||
|
||||
// Constructor //////////////////////////////////////////////////////////////
|
||||
|
||||
AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter) :
|
||||
RangeFinder(source, filter)
|
||||
{
|
||||
max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE;
|
||||
min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE;
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
{
|
||||
max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE;
|
||||
min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE;
|
||||
}
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
|
|
@ -1,16 +1,21 @@
|
|||
#ifndef AP_RangeFinder_SharpGP2Y_H
|
||||
#define AP_RangeFinder_SharpGP2Y_H
|
||||
|
||||
#include "RangeFinder.h"
|
||||
|
||||
#define AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE 20
|
||||
#define AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE 150
|
||||
|
||||
class AP_RangeFinder_SharpGP2Y : public RangeFinder
|
||||
{
|
||||
public:
|
||||
#ifndef AP_RangeFinder_SharpGP2Y_H
|
||||
#define AP_RangeFinder_SharpGP2Y_H
|
||||
|
||||
#include "RangeFinder.h"
|
||||
|
||||
#define AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE 20
|
||||
#define AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE 150
|
||||
|
||||
class AP_RangeFinder_SharpGP2Y : public RangeFinder
|
||||
{
|
||||
public:
|
||||
AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter);
|
||||
int convert_raw_to_distance(int _raw_value) { if( _raw_value == 0 ) return max_distance; else return 14500/_raw_value; } // read value from analog port and return distance in cm
|
||||
|
||||
};
|
||||
#endif
|
||||
int convert_raw_to_distance(int _raw_value) {
|
||||
if( _raw_value == 0 )
|
||||
return max_distance;
|
||||
else
|
||||
return 14500/_raw_value;
|
||||
} // read value from analog port and return distance in cm
|
||||
|
||||
};
|
||||
#endif
|
||||
|
|
|
@ -1,46 +1,45 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
|
||||
/*
|
||||
AP_RangeFinder.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
|
||||
infrared proximity sensor
|
||||
Code by Jose Julio and Randy Mackay. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This has the basic functions that all RangeFinders need implemented
|
||||
*/
|
||||
|
||||
// AVR LibC Includes
|
||||
#include "WConstants.h"
|
||||
#include "RangeFinder.h"
|
||||
|
||||
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
||||
void RangeFinder::set_orientation(int x, int y, int z)
|
||||
{
|
||||
orientation_x = x;
|
||||
orientation_y = y;
|
||||
orientation_z = z;
|
||||
}
|
||||
|
||||
// Read Sensor data - only the raw_value is filled in by this parent class
|
||||
int RangeFinder::read()
|
||||
{
|
||||
int temp_dist;
|
||||
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
|
||||
/*
|
||||
AP_RangeFinder.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
|
||||
infrared proximity sensor
|
||||
Code by Jose Julio and Randy Mackay. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This has the basic functions that all RangeFinders need implemented
|
||||
*/
|
||||
|
||||
// AVR LibC Includes
|
||||
#include "WConstants.h"
|
||||
#include "RangeFinder.h"
|
||||
|
||||
|
||||
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
|
||||
void RangeFinder::set_orientation(int x, int y, int z)
|
||||
{
|
||||
orientation_x = x;
|
||||
orientation_y = y;
|
||||
orientation_z = z;
|
||||
}
|
||||
|
||||
// Read Sensor data - only the raw_value is filled in by this parent class
|
||||
int RangeFinder::read()
|
||||
{
|
||||
int temp_dist;
|
||||
|
||||
raw_value = _analog_source->read();
|
||||
|
||||
// convert analog value to distance in cm (using child implementation most likely)
|
||||
temp_dist = convert_raw_to_distance(raw_value);
|
||||
|
||||
// ensure distance is within min and max
|
||||
temp_dist = constrain(temp_dist, min_distance, max_distance);
|
||||
|
||||
distance = _mode_filter->get_filtered_with_sample(temp_dist);
|
||||
return distance;
|
||||
}
|
||||
|
||||
// convert analog value to distance in cm (using child implementation most likely)
|
||||
temp_dist = convert_raw_to_distance(raw_value);
|
||||
|
||||
// ensure distance is within min and max
|
||||
temp_dist = constrain(temp_dist, min_distance, max_distance);
|
||||
|
||||
distance = _mode_filter->get_filtered_with_sample(temp_dist);
|
||||
return distance;
|
||||
}
|
||||
|
||||
|
|
|
@ -1,44 +1,43 @@
|
|||
#ifndef RangeFinder_h
|
||||
#define RangeFinder_h
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <inttypes.h>
|
||||
#ifndef RangeFinder_h
|
||||
#define RangeFinder_h
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <inttypes.h>
|
||||
#include "../AP_AnalogSource/AP_AnalogSource.h"
|
||||
#include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library
|
||||
|
||||
/*
|
||||
#define AP_RANGEFINDER_ORIENTATION_FRONT 0, 10, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_RIGHT -10, 0, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_BACK 0,-10, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_LEFT 10, 0, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_UP 0, 0,-10
|
||||
#define AP_RANGEFINDER_ORIENTATION_DOWN 0, 0, 10
|
||||
#define AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT -5, -5, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_BACK_RIGHT -5, -5, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_BACK_LEFT 5, -5, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_FRONT_LEFT 5, 5, 0
|
||||
*/
|
||||
|
||||
class RangeFinder
|
||||
{
|
||||
protected:
|
||||
#include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library
|
||||
|
||||
/*
|
||||
#define AP_RANGEFINDER_ORIENTATION_FRONT 0, 10, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_RIGHT -10, 0, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_BACK 0,-10, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_LEFT 10, 0, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_UP 0, 0,-10
|
||||
#define AP_RANGEFINDER_ORIENTATION_DOWN 0, 0, 10
|
||||
#define AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT -5, -5, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_BACK_RIGHT -5, -5, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_BACK_LEFT 5, -5, 0
|
||||
#define AP_RANGEFINDER_ORIENTATION_FRONT_LEFT 5, 5, 0
|
||||
*/
|
||||
|
||||
class RangeFinder
|
||||
{
|
||||
protected:
|
||||
RangeFinder(AP_AnalogSource * source, ModeFilter *filter) :
|
||||
_analog_source(source),
|
||||
_mode_filter(filter)
|
||||
{}
|
||||
public:
|
||||
|
||||
int raw_value; // raw value from the sensor
|
||||
int distance; // distance in cm
|
||||
int max_distance; // maximum measurable distance (in cm) - should be set in child's constructor
|
||||
int min_distance; // minimum measurable distance (in cm) - should be set in child's constructor
|
||||
int orientation_x, orientation_y, orientation_z;
|
||||
|
||||
virtual void set_orientation(int x, int y, int z);
|
||||
virtual int convert_raw_to_distance(int _raw_value) { return _raw_value; } // function that each child class should override to convert voltage to distance
|
||||
virtual int read(); // read value from sensor and return distance in cm
|
||||
|
||||
_mode_filter(filter) {}
|
||||
public:
|
||||
|
||||
int raw_value; // raw value from the sensor
|
||||
int distance; // distance in cm
|
||||
int max_distance; // maximum measurable distance (in cm) - should be set in child's constructor
|
||||
int min_distance; // minimum measurable distance (in cm) - should be set in child's constructor
|
||||
int orientation_x, orientation_y, orientation_z;
|
||||
|
||||
virtual void set_orientation(int x, int y, int z);
|
||||
virtual int convert_raw_to_distance(int _raw_value) { return _raw_value; } // function that each child class should override to convert voltage to distance
|
||||
virtual int read(); // read value from sensor and return distance in cm
|
||||
|
||||
AP_AnalogSource *_analog_source;
|
||||
ModeFilter *_mode_filter;
|
||||
};
|
||||
#endif
|
||||
ModeFilter *_mode_filter;
|
||||
};
|
||||
#endif
|
||||
|
|
|
@ -1,14 +1,14 @@
|
|||
RangeFinder KEYWORD1
|
||||
AP_RangeFinder KEYWORD1
|
||||
AP_RangeFinder_SharpGP2Y KEYWORD1
|
||||
AP_RangeFinder_MaxsonarXL KEYWORD1
|
||||
read KEYWORD2
|
||||
set_orientation KEYWORD2
|
||||
convert_raw_to_distance KEYWORD2
|
||||
raw_value KEYWORD2
|
||||
distance KEYWORD2
|
||||
max_distance KEYWORD2
|
||||
min_distance KEYWORD2
|
||||
orientation_x KEYWORD2
|
||||
orientation_y KEYWORD2
|
||||
orientation_z KEYWORD2
|
||||
RangeFinder KEYWORD1
|
||||
AP_RangeFinder KEYWORD1
|
||||
AP_RangeFinder_SharpGP2Y KEYWORD1
|
||||
AP_RangeFinder_MaxsonarXL KEYWORD1
|
||||
read KEYWORD2
|
||||
set_orientation KEYWORD2
|
||||
convert_raw_to_distance KEYWORD2
|
||||
raw_value KEYWORD2
|
||||
distance KEYWORD2
|
||||
max_distance KEYWORD2
|
||||
min_distance KEYWORD2
|
||||
orientation_x KEYWORD2
|
||||
orientation_y KEYWORD2
|
||||
orientation_z KEYWORD2
|
|
@ -135,7 +135,7 @@ static void sitl_fdm_input(void)
|
|||
|
||||
count++;
|
||||
if (millis() - last_report > 1000) {
|
||||
printf("SIM %u FPS\n", count);
|
||||
//printf("SIM %u FPS\n", count);
|
||||
count = 0;
|
||||
last_report = millis();
|
||||
}
|
||||
|
|
|
@ -111,11 +111,12 @@ void sitl_update_adc(float roll, float pitch, float yaw,
|
|||
(fabs(roll - dcm.roll_sensor/100.0) > 5.0 ||
|
||||
fabs(pitch - dcm.pitch_sensor/100.0) > 5.0))) {
|
||||
last_report = tnow;
|
||||
printf("roll=%5.1f / %5.1f pitch=%5.1f / %5.1f rr=%5.2f / %5.2f pr=%5.2f / %5.2f\n",
|
||||
/*printf("roll=%5.1f / %5.1f pitch=%5.1f / %5.1f rr=%5.2f / %5.2f pr=%5.2f / %5.2f\n",
|
||||
roll, dcm.roll_sensor/100.0,
|
||||
pitch, dcm.pitch_sensor/100.0,
|
||||
rollRate, ToDeg(omega.x),
|
||||
pitchRate, ToDeg(omega.y));
|
||||
*/
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue