Integration of optical flow
bug fixes for new Simple mode increased Baro filter by 2 fields
This commit is contained in:
Jason Short
parent
1025fc3abf
commit
d29cfbcee3
@ -55,8 +55,6 @@
|
||||
|
||||
// See the config.h and defines.h files for how to set this up!
|
||||
//
|
||||
// lets use SIMPLE mode for Roll and Pitch during Alt Hold
|
||||
//#define ALT_HOLD_RP ROLL_PITCH_SIMPLE
|
||||
|
||||
// lets use Manual throttle during Loiter
|
||||
//#define LOITER_THR THROTTLE_MANUAL
|
||||
|
||||
@ -61,7 +61,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List
|
||||
#include <APM_PI.h> // PI library
|
||||
#include <RC_Channel.h> // RC Channel Library
|
||||
#include <AP_RangeFinder.h> // Range finder library
|
||||
#include <AP_OpticalFlow.h> // Optical Flow library
|
||||
#include <AP_OpticalFlow_ADNS3080.h> // Optical Flow library
|
||||
#include <ModeFilter.h>
|
||||
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
||||
#include <memcheck.h>
|
||||
@ -127,9 +127,6 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
|
||||
APM_BMP085_Class barometer;
|
||||
AP_Compass_HMC5843 compass(Parameters::k_param_compass);
|
||||
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
AP_OpticalFlow_ADNS3080 optflow;
|
||||
#endif
|
||||
|
||||
// real GPS selection
|
||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||
@ -171,7 +168,7 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
|
||||
AP_Compass_HIL compass; // never used
|
||||
AP_IMU_Shim imu; // never used
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
AP_OpticalFlow_ADNS3080 optflow;
|
||||
AP_OpticalFlow_ADNS3080 optflow(&dcm);
|
||||
#endif
|
||||
static int32_t gps_base_alt;
|
||||
#else
|
||||
@ -201,6 +198,16 @@ static AP_Int8 *flight_modes = &g.flight_mode1;
|
||||
#endif
|
||||
// normal dcm
|
||||
AP_DCM dcm(&imu, g_gps);
|
||||
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
AP_OpticalFlow_ADNS3080 optflow(&dcm);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#elif HIL_MODE == HIL_MODE_ATTITUDE
|
||||
#ifdef OPTFLOW_ENABLED
|
||||
AP_OpticalFlow_ADNS3080 optflow(&dcm);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -467,15 +474,12 @@ static struct Location next_command; // command preloaded
|
||||
static struct Location guided_WP; // guided mode waypoint
|
||||
static long target_altitude; // used for
|
||||
static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location
|
||||
static struct Location optflow_offset; // optical flow base position
|
||||
static boolean new_location; // flag to tell us if location has been updated
|
||||
|
||||
|
||||
// IMU variables
|
||||
// -------------
|
||||
static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm)
|
||||
|
||||
|
||||
// Performance monitoring
|
||||
// ----------------------
|
||||
static long perf_mon_timer;
|
||||
@ -483,17 +487,10 @@ static long perf_mon_timer;
|
||||
static int G_Dt_max; // Max main loop cycle time in milliseconds
|
||||
static int gps_fix_count;
|
||||
|
||||
// GCS
|
||||
// ---
|
||||
//static char GCS_buffer[53];
|
||||
//static char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed
|
||||
|
||||
// System Timers
|
||||
// --------------
|
||||
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
|
||||
static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete
|
||||
static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds
|
||||
//static int mainLoop_count;
|
||||
|
||||
static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop
|
||||
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
|
||||
@ -514,6 +511,7 @@ static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS
|
||||
static byte counter_one_herz;
|
||||
static bool GPS_enabled = false;
|
||||
static byte loop_step;
|
||||
static bool new_radio_frame;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Top-level logic
|
||||
@ -537,7 +535,6 @@ void loop()
|
||||
// Execute the fast loop
|
||||
// ---------------------
|
||||
fast_loop();
|
||||
fast_loopTimeStamp = millis();
|
||||
}
|
||||
//PORTK &= B11101111;
|
||||
|
||||
@ -562,15 +559,12 @@ void loop()
|
||||
}
|
||||
|
||||
if (millis() - perf_mon_timer > 20000) {
|
||||
//if (mainLoop_count != 0) {
|
||||
|
||||
gcs.send_message(MSG_PERF_REPORT);
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_PM)
|
||||
Log_Write_Performance();
|
||||
|
||||
resetPerfData();
|
||||
//}
|
||||
}
|
||||
//PORTK &= B10111111;
|
||||
}
|
||||
@ -623,11 +617,15 @@ static void medium_loop()
|
||||
|
||||
medium_loopCounter++;
|
||||
|
||||
/*
|
||||
if(g.optflow_enabled){
|
||||
optflow.update()
|
||||
optflow.read();
|
||||
optflow.update_position(cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow
|
||||
|
||||
// write to log
|
||||
if (g.log_bitmask & MASK_LOG_OPTFLOW){
|
||||
Log_Write_Optflow();
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
if(GPS_enabled){
|
||||
update_GPS();
|
||||
@ -795,19 +793,8 @@ static void fifty_hz_loop()
|
||||
hil.send_message(MSG_RADIO_OUT);
|
||||
#endif
|
||||
|
||||
// use Yaw to find our bearing error
|
||||
//calc_bearing_error();
|
||||
|
||||
//if (throttle_slew < 0)
|
||||
// throttle_slew++;
|
||||
//else if (throttle_slew > 0)
|
||||
// throttle_slew--;
|
||||
|
||||
camera_stabilization();
|
||||
|
||||
|
||||
//throttle_slew = min((800 - g.rc_3.control_in), throttle_slew);
|
||||
|
||||
# if HIL_MODE == HIL_MODE_DISABLED
|
||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
|
||||
Log_Write_Attitude();
|
||||
@ -882,10 +869,10 @@ static void slow_loop()
|
||||
|
||||
#if AUTO_RESET_LOITER == 1
|
||||
if(control_mode == LOITER){
|
||||
if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){
|
||||
//if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 1500){
|
||||
// reset LOITER to current position
|
||||
//next_WP = current_loc;
|
||||
}
|
||||
//}
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -915,11 +902,6 @@ static void slow_loop()
|
||||
if(g.radio_tuning > 0)
|
||||
tuning();
|
||||
|
||||
// filter out the baro offset.
|
||||
//if(baro_alt_offset > 0) baro_alt_offset--;
|
||||
//if(baro_alt_offset < 0) baro_alt_offset++;
|
||||
|
||||
|
||||
#if MOTOR_LEDS == 1
|
||||
update_motor_leds();
|
||||
#endif
|
||||
@ -1005,6 +987,7 @@ static void update_GPS(void)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void update_yaw_mode(void)
|
||||
{
|
||||
switch(yaw_mode){
|
||||
@ -1054,28 +1037,19 @@ void update_roll_pitch_mode(void)
|
||||
|
||||
int control_roll = 0, control_pitch = 0;
|
||||
|
||||
if(do_simple){
|
||||
simple_timer++;
|
||||
if(simple_timer > 5){
|
||||
simple_timer = 0;
|
||||
//read_radio();
|
||||
|
||||
int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100;
|
||||
|
||||
// pre-calc rotation (stored in real degrees)
|
||||
// roll
|
||||
float cos_x = sin(radians(90 - delta));
|
||||
// pitch
|
||||
float sin_y = cos(radians(90 - delta));
|
||||
if(do_simple && new_radio_frame){
|
||||
new_radio_frame = false;
|
||||
//Serial.printf("1: %d, 2: %d ",g.rc_1.control_in, g.rc_2.control_in);
|
||||
|
||||
// Rotate input by the initial bearing
|
||||
// roll
|
||||
control_roll = g.rc_1.control_in * cos_x + g.rc_2.control_in * sin_y;
|
||||
// pitch
|
||||
control_pitch = -(g.rc_1.control_in * sin_y - g.rc_2.control_in * cos_x);
|
||||
control_roll = g.rc_1.control_in * sin_yaw_y + g.rc_2.control_in * cos_yaw_x;
|
||||
control_pitch = g.rc_2.control_in * sin_yaw_y - g.rc_1.control_in * cos_yaw_x;
|
||||
|
||||
g.rc_1.control_in = control_roll;
|
||||
g.rc_2.control_in = control_pitch;
|
||||
}
|
||||
//Serial.printf("\t1: %d, 2: %d\n",g.rc_1.control_in, g.rc_2.control_in);
|
||||
}
|
||||
|
||||
switch(roll_pitch_mode){
|
||||
@ -1085,29 +1059,26 @@ void update_roll_pitch_mode(void)
|
||||
|
||||
// Pitch control
|
||||
g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in);
|
||||
return;
|
||||
break;
|
||||
|
||||
case ROLL_PITCH_STABLE:
|
||||
control_roll = g.rc_1.control_in;
|
||||
control_pitch = g.rc_2.control_in;
|
||||
g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in);
|
||||
g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
|
||||
break;
|
||||
|
||||
case ROLL_PITCH_AUTO:
|
||||
// mix in user control with Nav control
|
||||
control_roll = g.rc_1.control_mix(nav_roll);
|
||||
control_pitch = g.rc_2.control_mix(nav_pitch);
|
||||
g.rc_1.servo_out = get_stabilize_roll(control_roll);
|
||||
g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
|
||||
break;
|
||||
}
|
||||
|
||||
// Roll control
|
||||
g.rc_1.servo_out = get_stabilize_roll(control_roll);
|
||||
|
||||
// Pitch control
|
||||
g.rc_2.servo_out = get_stabilize_pitch(control_pitch);
|
||||
}
|
||||
|
||||
|
||||
// 50 hz update rate, not 250
|
||||
void update_throttle_mode(void)
|
||||
{
|
||||
switch(throttle_mode){
|
||||
@ -1238,8 +1209,6 @@ static void update_trig(void){
|
||||
yawvector.y = temp.b.x; // cos
|
||||
yawvector.normalize();
|
||||
|
||||
cos_yaw_x = yawvector.y; // 0 x = north
|
||||
sin_yaw_y = yawvector.x; // 1 y
|
||||
|
||||
sin_pitch_y = -temp.c.x;
|
||||
cos_pitch_x = sqrt(1 - (temp.c.x * temp.c.x));
|
||||
@ -1247,6 +1216,9 @@ static void update_trig(void){
|
||||
cos_roll_x = temp.c.z / cos_pitch_x;
|
||||
sin_roll_y = temp.c.y / cos_pitch_x;
|
||||
|
||||
cos_yaw_x = yawvector.y; // 0 x = north
|
||||
sin_yaw_y = yawvector.x; // 1 y
|
||||
|
||||
//flat:
|
||||
// 0 ° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 0.00, sy: 1.00,
|
||||
// 90° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 1.00, sy: 0.00,
|
||||
@ -1366,8 +1338,8 @@ static void tuning(){
|
||||
|
||||
case CH6_RELAY:
|
||||
g.rc_6.set_range(0,1000);
|
||||
if (g.rc_6.control_in <= 600) relay_on();
|
||||
if (g.rc_6.control_in >= 400) relay_off();
|
||||
if (g.rc_6.control_in > 525) relay_on();
|
||||
if (g.rc_6.control_in < 475) relay_off();
|
||||
break;
|
||||
|
||||
case CH6_TRAVERSE_SPEED:
|
||||
|
||||
@ -91,7 +91,6 @@ static int
|
||||
get_nav_throttle(long z_error, int target_speed)
|
||||
{
|
||||
int rate_error;
|
||||
//int throttle;
|
||||
float scaler = (float)target_speed/(float)ALT_ERROR_MAX;
|
||||
|
||||
// limit error to prevent I term run up
|
||||
@ -101,14 +100,9 @@ get_nav_throttle(long z_error, int target_speed)
|
||||
rate_error = target_speed - altitude_rate;
|
||||
rate_error = constrain(rate_error, -110, 110);
|
||||
|
||||
//throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
|
||||
//return g.throttle_cruise + throttle;
|
||||
|
||||
|
||||
return g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop);
|
||||
}
|
||||
|
||||
|
||||
static int
|
||||
get_rate_roll(long target_rate)
|
||||
{
|
||||
|
||||
@ -98,9 +98,6 @@ dump_log(uint8_t argc, const Menu::arg *argv)
|
||||
int dump_log_start;
|
||||
int dump_log_end;
|
||||
|
||||
//byte last_log_num = get_num_logs();
|
||||
//Serial.printf_P(PSTR("\n%d logs\n"), last_log_num);
|
||||
|
||||
// check that the requested log number can be read
|
||||
dump_log = argv[1].i;
|
||||
|
||||
@ -506,8 +503,8 @@ static void Log_Write_Optflow()
|
||||
DataFlash.WriteInt((int)optflow.dx);
|
||||
DataFlash.WriteInt((int)optflow.dy);
|
||||
DataFlash.WriteInt((int)optflow.surface_quality);
|
||||
DataFlash.WriteLong(optflow.lat);//optflow_offset.lat + optflow.lat);
|
||||
DataFlash.WriteLong(optflow.lng);//optflow_offset.lng + optflow.lng);
|
||||
DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat);
|
||||
DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -462,12 +462,7 @@ static void do_change_alt()
|
||||
Location temp = next_WP;
|
||||
condition_start = current_loc.alt;
|
||||
condition_value = next_command.alt;
|
||||
//if (next_command.options & WP_OPTION_ALT_RELATIVE) {
|
||||
// condition_value = next_command.alt + home.alt;
|
||||
//} else {
|
||||
|
||||
//}
|
||||
temp.alt = condition_value;
|
||||
temp.alt = next_command.alt;
|
||||
set_next_WP(&temp);
|
||||
}
|
||||
|
||||
|
||||
@ -207,9 +207,11 @@
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// OPTICAL_FLOW
|
||||
#if defined( __AVR_ATmega2560__ ) // determines if optical flow code is included
|
||||
//#define OPTFLOW_ENABLED
|
||||
#define OPTFLOW_ENABLED
|
||||
#endif
|
||||
|
||||
#define OPTFLOW_ENABLED
|
||||
|
||||
#ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI)
|
||||
# define OPTFLOW DISABLED
|
||||
#endif
|
||||
@ -310,7 +312,7 @@
|
||||
#endif
|
||||
|
||||
#ifndef SIMPLE_RP
|
||||
# define SIMPLE_RP ROLL_PITCH_SIMPLE
|
||||
# define SIMPLE_RP ROLL_PITCH_STABLE
|
||||
#endif
|
||||
|
||||
#ifndef SIMPLE_THR
|
||||
@ -413,7 +415,7 @@
|
||||
// Rate Control
|
||||
//
|
||||
#ifndef RATE_ROLL_P
|
||||
# define RATE_ROLL_P .13
|
||||
# define RATE_ROLL_P 0.13
|
||||
#endif
|
||||
#ifndef RATE_ROLL_I
|
||||
# define RATE_ROLL_I 0.0
|
||||
@ -488,22 +490,6 @@
|
||||
# define NAV_IMAX 20 // degrees
|
||||
#endif
|
||||
|
||||
/*
|
||||
#ifndef NAV_LOITER_P
|
||||
# define NAV_LOITER_P .4 //1.4 //
|
||||
#endif
|
||||
#ifndef NAV_LOITER_I
|
||||
# define NAV_LOITER_I 0.05 //
|
||||
#endif
|
||||
#ifndef NAV_LOITER_D
|
||||
# define NAV_LOITER_D 2 //
|
||||
#endif
|
||||
#ifndef NAV_LOITER_IMAX
|
||||
# define NAV_LOITER_IMAX 8 // degrees°
|
||||
#endif
|
||||
*/
|
||||
|
||||
|
||||
#ifndef WAYPOINT_SPEED_MAX
|
||||
# define WAYPOINT_SPEED_MAX 450 // for 6m/s error = 13mph
|
||||
#endif
|
||||
@ -518,9 +504,6 @@
|
||||
#ifndef THROTTLE_I
|
||||
# define THROTTLE_I 0.10 // with 4m error, 12.5s windup
|
||||
#endif
|
||||
//#ifndef THROTTLE_D
|
||||
//# define THROTTLE_D 0.6 // upped with filter
|
||||
//#endif
|
||||
#ifndef THROTTLE_IMAX
|
||||
# define THROTTLE_IMAX 300
|
||||
#endif
|
||||
|
||||
@ -14,10 +14,13 @@ static void read_control_switch()
|
||||
|
||||
set_mode(flight_modes[switchPosition]);
|
||||
|
||||
#if CH7_OPTION != SIMPLE_MODE_CONTROL
|
||||
// setup Simple mode
|
||||
// do we enable simple mode?
|
||||
do_simple = (g.simple_modes & 1 << switchPosition);
|
||||
Serial.printf("do simple: %d \n", (int)do_simple);
|
||||
//Serial.printf("do simple: %d \n", (int)do_simple);
|
||||
#endif
|
||||
|
||||
|
||||
}else{
|
||||
switch_debouncer = true;
|
||||
@ -56,6 +59,7 @@ static void read_trim_switch()
|
||||
#elif CH7_OPTION == SIMPLE_MODE_CONTROL
|
||||
|
||||
do_simple = (g.rc_7.control_in > 800);
|
||||
//Serial.println(g.rc_7.control_in, DEC);
|
||||
|
||||
#elif CH7_OPTION == DO_SET_HOVER
|
||||
// switch is engaged
|
||||
|
||||
@ -14,8 +14,7 @@
|
||||
|
||||
#define ROLL_PITCH_STABLE 0
|
||||
#define ROLL_PITCH_ACRO 1
|
||||
#define ROLL_PITCH_SIMPLE 2
|
||||
#define ROLL_PITCH_AUTO 3
|
||||
#define ROLL_PITCH_AUTO 2
|
||||
|
||||
#define THROTTLE_MANUAL 0
|
||||
#define THROTTLE_HOLD 1
|
||||
|
||||
@ -30,16 +30,6 @@ static void navigate()
|
||||
// target_bearing is where we should be heading
|
||||
// --------------------------------------------
|
||||
target_bearing = get_bearing(¤t_loc, &next_WP);
|
||||
|
||||
// nav_bearing will include xtrac correction
|
||||
// -----------------------------------------
|
||||
//xtrack_enabled = false;
|
||||
|
||||
//if(xtrack_enabled){
|
||||
// crosstrack_correction = get_crosstrack_correction();
|
||||
//}else {
|
||||
// crosstrack_correction = 0;
|
||||
//}
|
||||
}
|
||||
|
||||
static bool check_missed_wp()
|
||||
@ -71,6 +61,9 @@ static void calc_location_error(struct Location *next_loc)
|
||||
lat_error = next_loc->lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH
|
||||
}
|
||||
|
||||
|
||||
// nav_roll = g.pid_of_roll.get_pid(-optflow.x_cm * 10, dTnav, 1.0);
|
||||
|
||||
#define NAV_ERR_MAX 400
|
||||
static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed)
|
||||
{
|
||||
@ -105,15 +98,20 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed
|
||||
float temp = radians((float)g_gps->ground_course/100.0);
|
||||
|
||||
// calc the cos of the error to tell how fast we are moving towards the target in cm
|
||||
if(g.optflow_enabled && current_loc.alt < 500 && g_gps->ground_speed < 150){
|
||||
x_actual_speed = optflow.vlon * 10;
|
||||
y_actual_speed = optflow.vlat * 10;
|
||||
}else{
|
||||
x_actual_speed = (float)g_gps->ground_speed * sin(temp);
|
||||
y_actual_speed = (float)g_gps->ground_speed * cos(temp);
|
||||
}
|
||||
|
||||
y_rate_error = y_target_speed - y_actual_speed; // 413
|
||||
y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum
|
||||
nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500);
|
||||
|
||||
//Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator());
|
||||
|
||||
// calc the sin of the error to tell how fast we are moving laterally to the target in cm
|
||||
x_actual_speed = (float)g_gps->ground_speed * sin(temp);
|
||||
x_rate_error = x_target_speed - x_actual_speed;
|
||||
x_rate_error = constrain(x_rate_error, -600, 600);
|
||||
nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500);
|
||||
|
||||
@ -101,7 +101,7 @@ void output_min()
|
||||
static void read_radio()
|
||||
{
|
||||
if (APM_RC.GetState() == 1){
|
||||
|
||||
new_radio_frame = true;
|
||||
g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
|
||||
g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
|
||||
g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
|
||||
|
||||
@ -304,7 +304,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
byte _oldSwitchPosition = 0;
|
||||
byte mode = 0;
|
||||
|
||||
Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off"));
|
||||
Serial.printf_P(PSTR("\nMove mode switch to edit, aileron: select modes, rudder: Simple on/off\n"));
|
||||
print_hit_enter();
|
||||
|
||||
while(1){
|
||||
@ -341,7 +341,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
|
||||
// look for stick input
|
||||
if (abs(g.rc_4.control_in) > 3000){
|
||||
if (g.rc_4.control_in > 3000){
|
||||
g.simple_modes |= (1<<_switchPosition);
|
||||
// print new mode
|
||||
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
|
||||
@ -349,7 +349,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
|
||||
// look for stick input
|
||||
if (abs(g.rc_4.control_in) < 3000){
|
||||
if (g.rc_4.control_in < -3000){
|
||||
g.simple_modes &= ~(1<<_switchPosition);
|
||||
// print new mode
|
||||
print_switch(_switchPosition, mode, (g.simple_modes & (1<<_switchPosition)));
|
||||
@ -361,6 +361,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
for (mode = 0; mode < 6; mode++)
|
||||
flight_modes[mode].save();
|
||||
|
||||
g.simple_modes.save();
|
||||
print_done();
|
||||
report_flight_modes();
|
||||
return (0);
|
||||
@ -760,10 +761,6 @@ setup_optflow(uint8_t argc, const Menu::arg *argv)
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
||||
g.optflow_enabled = false;
|
||||
|
||||
//} else if(argv[1].i > 10){
|
||||
// g.optflow_fov.set_and_save(argv[1].i);
|
||||
// optflow.set_field_of_view(g.optflow_fov.get());
|
||||
|
||||
}else{
|
||||
Serial.printf_P(PSTR("\nOptions:[on, off]\n"));
|
||||
report_optflow();
|
||||
@ -978,13 +975,13 @@ print_radio_values()
|
||||
static void
|
||||
print_switch(byte p, byte m, bool b)
|
||||
{
|
||||
Serial.printf_P(PSTR("Pos %d: "),p);
|
||||
Serial.printf_P(PSTR("Pos %d:\t"),p);
|
||||
Serial.print(flight_mode_strings[m]);
|
||||
Serial.printf_P(PSTR(", Simple: "));
|
||||
Serial.printf_P(PSTR(",\t\tSimple: "));
|
||||
if(b)
|
||||
Serial.printf_P(PSTR("T\n"));
|
||||
Serial.printf_P(PSTR("ON\n"));
|
||||
else
|
||||
Serial.printf_P(PSTR("F\n"));
|
||||
Serial.printf_P(PSTR("OFF\n"));
|
||||
}
|
||||
|
||||
static void
|
||||
|
||||
@ -321,8 +321,7 @@ static void startup_ground(void)
|
||||
|
||||
#define ROLL_PITCH_STABLE 0
|
||||
#define ROLL_PITCH_ACRO 1
|
||||
#define ROLL_PITCH_SIMPLE 2
|
||||
#define ROLL_PITCH_AUTO 3
|
||||
#define ROLL_PITCH_AUTO 2
|
||||
|
||||
#define THROTTLE_MANUAL 0
|
||||
#define THROTTLE_HOLD 1
|
||||
@ -489,7 +488,10 @@ init_compass()
|
||||
static void
|
||||
init_optflow()
|
||||
{
|
||||
optflow.init();
|
||||
if( optflow.init() == false ) {
|
||||
g.optflow_enabled = false;
|
||||
//SendDebug("\nFailed to Init OptFlow ");
|
||||
}
|
||||
optflow.set_orientation(OPTFLOW_ORIENTATION); // set optical flow sensor's orientation on aircraft
|
||||
optflow.set_field_of_view(OPTFLOW_FOV); // set optical flow sensor's field of view
|
||||
}
|
||||
|
||||
@ -871,6 +871,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
|
||||
static int8_t
|
||||
test_optflow(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
///*
|
||||
if(g.optflow_enabled) {
|
||||
Serial.printf_P(PSTR("man id: %d\t"),optflow.read_register(ADNS3080_PRODUCT_ID));
|
||||
print_hit_enter();
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
#define APM_BMP085_h
|
||||
|
||||
#define TEMP_FILTER_SIZE 16
|
||||
#define PRESS_FILTER_SIZE 8
|
||||
#define PRESS_FILTER_SIZE 10
|
||||
|
||||
class APM_BMP085_Class
|
||||
{
|
||||
|
||||
@ -79,15 +79,15 @@ AP_RC_Channel::set_pwm(int pwm)
|
||||
//Serial.print("range ");
|
||||
control_in = pwm_to_range();
|
||||
control_in = (control_in < dead_zone) ? 0 : control_in;
|
||||
if (fabs(scale_output) > 0){
|
||||
control_in *= scale_output;
|
||||
}
|
||||
|
||||
}else{
|
||||
control_in = pwm_to_angle();
|
||||
control_in = (abs(control_in) < dead_zone) ? 0 : control_in;
|
||||
if (fabs(scale_output) > 0){
|
||||
control_in *= scale_output;
|
||||
|
||||
if(expo) {
|
||||
long temp = control_in;
|
||||
temp = (temp * temp) / (long)_high;
|
||||
control_in = (int)((control_in >= 0) ? temp : -temp);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -23,17 +23,13 @@ class AP_RC_Channel{
|
||||
_high(1),
|
||||
_filter(true),
|
||||
_reverse(1),
|
||||
dead_zone(0),
|
||||
scale_output(1.0)
|
||||
{}
|
||||
dead_zone(0){}
|
||||
|
||||
AP_RC_Channel() :
|
||||
_high(1),
|
||||
_filter(true),
|
||||
_reverse(1),
|
||||
dead_zone(0),
|
||||
scale_output(1.0)
|
||||
{}
|
||||
dead_zone(0){}
|
||||
|
||||
// setup min and max radio values in CLI
|
||||
void update_min_max();
|
||||
@ -94,8 +90,6 @@ class AP_RC_Channel{
|
||||
int16_t pwm_to_range();
|
||||
int16_t range_to_pwm();
|
||||
|
||||
float scale_output;
|
||||
|
||||
private:
|
||||
bool _filter;
|
||||
int8_t _reverse;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user