Copter: TOY mode updates
This commit is contained in:
parent
55b22f8635
commit
60775e94b9
@ -141,7 +141,7 @@
|
||||
static AP_HAL::BetterStream* cliSerial;
|
||||
|
||||
// N.B. we need to keep a static declaration which isn't guarded by macros
|
||||
// at the top to cooperate with the prototype mangler.
|
||||
// at the top to cooperate with the prototype mangler.
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// AP_HAL instance
|
||||
@ -644,8 +644,6 @@ static float target_sonar_alt; // desired altitude in cm above the ground
|
||||
// The altitude as reported by Baro in cm – Values can be quite high
|
||||
static int32_t baro_alt;
|
||||
|
||||
static int16_t saved_toy_throttle;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// flight modes
|
||||
@ -714,7 +712,6 @@ static uint32_t throttle_integrator;
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The Commanded Yaw from the autopilot.
|
||||
static int32_t nav_yaw;
|
||||
static uint8_t yaw_timer;
|
||||
// Yaw will point at this location if yaw_mode is set to YAW_LOOK_AT_LOCATION
|
||||
static Vector3f yaw_look_at_WP;
|
||||
// bearing from current location to the yaw_look_at_WP
|
||||
@ -863,7 +860,6 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
|
||||
{ read_aux_switches, 10, 50 },
|
||||
{ arm_motors_check, 10, 10 },
|
||||
{ auto_trim, 10, 140 },
|
||||
{ update_toy_throttle, 10, 50 },
|
||||
{ update_altitude, 10, 1000 },
|
||||
{ run_nav_updates, 10, 800 },
|
||||
{ three_hz_loop, 33, 90 },
|
||||
@ -947,7 +943,7 @@ static void compass_accumulate(void)
|
||||
{
|
||||
if (g.compass_enabled) {
|
||||
compass.accumulate();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
@ -963,7 +959,7 @@ static void perf_update(void)
|
||||
if (g.log_bitmask & MASK_LOG_PM)
|
||||
Log_Write_Performance();
|
||||
if (scheduler.debug()) {
|
||||
cliSerial->printf_P(PSTR("PERF: %u/%u %lu\n"),
|
||||
cliSerial->printf_P(PSTR("PERF: %u/%u %lu\n"),
|
||||
(unsigned)perf_info_get_num_long_running(),
|
||||
(unsigned)perf_info_get_num_loops(),
|
||||
(unsigned long)perf_info_get_max_time());
|
||||
@ -1079,10 +1075,6 @@ static void throttle_loop()
|
||||
// check if we've landed
|
||||
update_land_detector();
|
||||
|
||||
#if TOY_EDF == ENABLED
|
||||
edf_toy();
|
||||
#endif
|
||||
|
||||
// check auto_armed status
|
||||
update_auto_armed();
|
||||
}
|
||||
@ -1318,7 +1310,7 @@ static void update_GPS(void)
|
||||
if (camera.update_location(current_loc) == true) {
|
||||
do_take_picture();
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
// check for loss of gps
|
||||
@ -1497,19 +1489,13 @@ void update_yaw_mode(void)
|
||||
get_look_ahead_yaw(g.rc_4.control_in);
|
||||
break;
|
||||
|
||||
#if TOY_LOOKUP == TOY_EXTERNAL_MIXER
|
||||
case YAW_TOY:
|
||||
// if we are landed reset yaw target to current heading
|
||||
if (ap.land_complete) {
|
||||
nav_yaw = ahrs.yaw_sensor;
|
||||
}else{
|
||||
// update to allow external roll/yaw mixing
|
||||
// keep heading always pointing at home with no pilot input allowed
|
||||
nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE);
|
||||
}
|
||||
get_stabilize_yaw(nav_yaw);
|
||||
get_yaw_toy();
|
||||
break;
|
||||
#endif
|
||||
|
||||
case YAW_RESETTOARMEDYAW:
|
||||
// if we are landed reset yaw target to current heading
|
||||
@ -1543,7 +1529,7 @@ uint8_t get_wp_yaw_mode(bool rtl)
|
||||
if( rtl ) {
|
||||
return YAW_HOLD;
|
||||
}else{
|
||||
return YAW_LOOK_AT_NEXT_WP;
|
||||
return YAW_LOOK_AT_NEXT_WP;
|
||||
}
|
||||
break;
|
||||
|
||||
@ -1690,10 +1676,8 @@ void update_roll_pitch_mode(void)
|
||||
get_stabilize_pitch(get_of_pitch(control_pitch));
|
||||
break;
|
||||
|
||||
// THOR
|
||||
// a call out to the main toy logic
|
||||
case ROLL_PITCH_TOY:
|
||||
roll_pitch_toy();
|
||||
get_roll_pitch_toy();
|
||||
break;
|
||||
|
||||
case ROLL_PITCH_LOITER:
|
||||
@ -1885,7 +1869,7 @@ void update_throttle_mode(void)
|
||||
} else {
|
||||
motors.stab_throttle = false;
|
||||
}
|
||||
|
||||
|
||||
// allow swash collective to move if we are in manual throttle modes, even if disarmed
|
||||
if( !motors.armed() ) {
|
||||
if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){
|
||||
@ -1903,7 +1887,7 @@ void update_throttle_mode(void)
|
||||
set_target_alt_for_reporting(0);
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
#endif // HELI_FRAME
|
||||
|
||||
switch(throttle_mode) {
|
||||
|
@ -294,42 +294,42 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Param: FLTMODE1
|
||||
// @DisplayName: Flight Mode 1
|
||||
// @Description: Flight mode when Channel 5 pwm is <= 1230
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
|
||||
|
||||
// @Param: FLTMODE2
|
||||
// @DisplayName: Flight Mode 2
|
||||
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
|
||||
|
||||
// @Param: FLTMODE3
|
||||
// @DisplayName: Flight Mode 3
|
||||
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
|
||||
|
||||
// @Param: FLTMODE4
|
||||
// @DisplayName: Flight Mode 4
|
||||
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
|
||||
|
||||
// @Param: FLTMODE5
|
||||
// @DisplayName: Flight Mode 5
|
||||
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
|
||||
|
||||
// @Param: FLTMODE6
|
||||
// @DisplayName: Flight Mode 6
|
||||
// @Description: Flight mode when Channel 5 pwm is >=1750
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
|
||||
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:Toy,13:Sport
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),
|
||||
|
||||
|
@ -99,12 +99,6 @@
|
||||
#ifndef FRAME_ORIENTATION
|
||||
# define FRAME_ORIENTATION X_FRAME
|
||||
#endif
|
||||
#ifndef TOY_EDF
|
||||
# define TOY_EDF DISABLED
|
||||
#endif
|
||||
#ifndef TOY_MIXER
|
||||
# define TOY_MIXER TOY_LINEAR_MIXER
|
||||
#endif
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////
|
||||
// TradHeli defaults
|
||||
|
@ -144,11 +144,11 @@
|
||||
#define POSITION 8 // AUTO control
|
||||
#define LAND 9 // AUTO control
|
||||
#define OF_LOITER 10 // Hold a single location using optical flow sensor
|
||||
#define TOY_A 11 // THOR Enum for Toy mode
|
||||
#define TOY_M 12 // THOR Enum for Toy mode
|
||||
#define TOY 11 // THOR Enum for Toy mode (Note: 12 is no longer used)
|
||||
#define SPORT 13 // earth frame rate control
|
||||
#define NUM_MODES 14
|
||||
|
||||
|
||||
// CH_6 Tuning
|
||||
// -----------
|
||||
#define CH6_NONE 0 // no tuning performed
|
||||
|
@ -33,7 +33,7 @@ static void arm_motors_check()
|
||||
arming_counter = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
if ((motors.rsc_mode > 0) && (g.rc_8.control_in >= 10)){
|
||||
arming_counter = 0;
|
||||
@ -41,11 +41,7 @@ static void arm_motors_check()
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
|
||||
#if TOY_EDF == ENABLED
|
||||
int16_t tmp = g.rc_1.control_in;
|
||||
#else
|
||||
int16_t tmp = g.rc_4.control_in;
|
||||
#endif
|
||||
|
||||
// full right
|
||||
if (tmp > 4000) {
|
||||
@ -233,7 +229,7 @@ static void pre_arm_checks(bool display_failure)
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// pre-arm check to ensure ch7 and ch8 have different functions
|
||||
if ((g.ch7_option != 0 || g.ch8_option != 0) && g.ch7_option == g.ch8_option) {
|
||||
if (display_failure) {
|
||||
|
@ -28,7 +28,7 @@ static void calc_position(){
|
||||
static void calc_distance_and_bearing()
|
||||
{
|
||||
Vector3f curr = inertial_nav.get_position();
|
||||
|
||||
|
||||
// get target from loiter or wpinav controller
|
||||
if( nav_mode == NAV_LOITER || nav_mode == NAV_CIRCLE ) {
|
||||
wp_distance = wp_nav.get_distance_to_target();
|
||||
@ -164,16 +164,6 @@ static void update_nav_mode()
|
||||
log_counter = 0;
|
||||
Log_Write_Nav_Tuning();
|
||||
}
|
||||
|
||||
/*
|
||||
// To-Do: check that we haven't broken toy mode
|
||||
case TOY_A:
|
||||
case TOY_M:
|
||||
set_nav_mode(NAV_NONE);
|
||||
update_nav_wp();
|
||||
break;
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
// Keeps old data out of our calculation / logs
|
||||
@ -225,7 +215,7 @@ circle_set_center(const Vector3f current_position, float heading_in_radians)
|
||||
circle_angle = wrap_PI(heading_in_radians-PI);
|
||||
|
||||
// calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle
|
||||
max_velocity = min(wp_nav.get_horizontal_velocity(), safe_sqrt(0.5f*wp_nav.get_waypoint_acceleration()*g.circle_radius*100.0f));
|
||||
max_velocity = min(wp_nav.get_horizontal_velocity(), safe_sqrt(0.5f*wp_nav.get_waypoint_acceleration()*g.circle_radius*100.0f));
|
||||
|
||||
// angular_velocity in radians per second
|
||||
circle_angular_velocity_max = max_velocity/((float)g.circle_radius * 100.0f);
|
||||
|
@ -92,12 +92,6 @@ static void init_rc_out()
|
||||
if (ap.pre_arm_rc_check) {
|
||||
output_min();
|
||||
}
|
||||
|
||||
#if TOY_EDF == ENABLED
|
||||
// add access to CH8 and CH6
|
||||
APM_RC.enable_out(CH_8);
|
||||
APM_RC.enable_out(CH_6);
|
||||
#endif
|
||||
}
|
||||
|
||||
// output_min - enable and output lowest possible value to motors
|
||||
|
@ -63,7 +63,7 @@ static void run_cli(AP_HAL::UARTDriver *port)
|
||||
motors.armed(false);
|
||||
motors.output();
|
||||
}
|
||||
|
||||
|
||||
while (1) {
|
||||
main_menu.run();
|
||||
}
|
||||
@ -120,7 +120,7 @@ static void init_ardupilot()
|
||||
//
|
||||
report_version();
|
||||
|
||||
relay.init();
|
||||
relay.init();
|
||||
|
||||
#if COPTER_LEDS == ENABLED
|
||||
copter_leds_init();
|
||||
@ -142,14 +142,14 @@ static void init_ardupilot()
|
||||
hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
|
||||
|
||||
// we start by assuming USB connected, as we initialed the serial
|
||||
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
|
||||
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
|
||||
ap.usb_connected = true;
|
||||
check_usb_mux();
|
||||
|
||||
#if CONFIG_HAL_BOARD != HAL_BOARD_APM2
|
||||
// we have a 2nd serial port for telemetry on all boards except
|
||||
// APM2. We actually do have one on APM2 but it isn't necessary as
|
||||
// a MUX is used
|
||||
// a MUX is used
|
||||
hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
|
||||
gcs3.init(hal.uartC);
|
||||
#endif
|
||||
@ -303,14 +303,14 @@ static bool mode_requires_GPS(uint8_t mode) {
|
||||
switch(mode) {
|
||||
case AUTO:
|
||||
case GUIDED:
|
||||
case LOITER:
|
||||
case LOITER:
|
||||
case RTL:
|
||||
case CIRCLE:
|
||||
case POSITION:
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
@ -320,8 +320,7 @@ static bool manual_flight_mode(uint8_t mode) {
|
||||
switch(mode) {
|
||||
case ACRO:
|
||||
case STABILIZE:
|
||||
case TOY_A:
|
||||
case TOY_M:
|
||||
case TOY:
|
||||
case SPORT:
|
||||
return true;
|
||||
default:
|
||||
@ -441,21 +440,7 @@ static bool set_mode(uint8_t mode)
|
||||
}
|
||||
break;
|
||||
|
||||
// THOR
|
||||
// These are the flight modes for Toy mode
|
||||
// See the defines for the enumerated values
|
||||
case TOY_A:
|
||||
success = true;
|
||||
set_yaw_mode(YAW_TOY);
|
||||
set_roll_pitch_mode(ROLL_PITCH_TOY);
|
||||
set_throttle_mode(THROTTLE_AUTO);
|
||||
set_nav_mode(NAV_NONE);
|
||||
|
||||
// save throttle for fast exit of Alt hold
|
||||
saved_toy_throttle = g.rc_3.control_in;
|
||||
break;
|
||||
|
||||
case TOY_M:
|
||||
case TOY:
|
||||
success = true;
|
||||
set_yaw_mode(YAW_TOY);
|
||||
set_roll_pitch_mode(ROLL_PITCH_TOY);
|
||||
@ -607,11 +592,8 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
||||
case OF_LOITER:
|
||||
port->print_P(PSTR("OF_LOITER"));
|
||||
break;
|
||||
case TOY_M:
|
||||
port->print_P(PSTR("TOY_M"));
|
||||
break;
|
||||
case TOY_A:
|
||||
port->print_P(PSTR("TOY_A"));
|
||||
case TOY:
|
||||
port->print_P(PSTR("TOY"));
|
||||
break;
|
||||
case SPORT:
|
||||
port->print_P(PSTR("SPORT"));
|
||||
|
@ -1,175 +1,31 @@
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Toy Mode - THOR
|
||||
// Toy Mode
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
static bool CH7_toy_flag;
|
||||
|
||||
#if TOY_MIXER == TOY_LOOKUP_TABLE
|
||||
static const int16_t toy_lookup[] = {
|
||||
186, 373, 558, 745,
|
||||
372, 745, 1117, 1490,
|
||||
558, 1118, 1675, 2235,
|
||||
743, 1490, 2233, 2980,
|
||||
929, 1863, 2792, 3725,
|
||||
1115, 2235, 3350, 4470,
|
||||
1301, 2608, 3908, 4500,
|
||||
1487, 2980, 4467, 4500,
|
||||
1673, 3353, 4500, 4500
|
||||
};
|
||||
#endif
|
||||
static float toy_gain;
|
||||
|
||||
//called at 10hz
|
||||
void update_toy_throttle()
|
||||
static void
|
||||
get_yaw_toy()
|
||||
{
|
||||
if(control_mode == TOY_A) {
|
||||
// look for a change in throttle position to exit throttle hold
|
||||
if(abs(g.rc_3.control_in - saved_toy_throttle) > 40) {
|
||||
throttle_mode = THROTTLE_MANUAL;
|
||||
}
|
||||
// convert pilot input to lean angles
|
||||
// moved to Yaw since it is called before get_roll_pitch_toy();
|
||||
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch);
|
||||
|
||||
if(throttle_mode == THROTTLE_AUTO) {
|
||||
update_toy_altitude();
|
||||
}
|
||||
}
|
||||
// Gain scheduling for Yaw input -
|
||||
// we reduce the yaw input based on the velocity of the copter
|
||||
// 0 = full control, 600cm/s = 20% control
|
||||
toy_gain = min(inertial_nav.get_velocity_xy(), 600);
|
||||
toy_gain = 1.0 - (toy_gain / 750.0);
|
||||
get_yaw_rate_stabilized_ef((float)control_roll * toy_gain);
|
||||
}
|
||||
|
||||
#define TOY_ALT_SMALL 25
|
||||
#define TOY_ALT_LARGE 100
|
||||
|
||||
//called at 10hz
|
||||
void update_toy_altitude()
|
||||
{
|
||||
int16_t input = g.rc_3.radio_in; // throttle
|
||||
float current_target_alt = wp_nav.get_desired_alt();
|
||||
//int16_t input = g.rc_7.radio_in;
|
||||
|
||||
// Trigger upward alt change
|
||||
if(false == CH7_toy_flag && input > 1666) {
|
||||
CH7_toy_flag = true;
|
||||
// go up
|
||||
if(current_target_alt >= 400) {
|
||||
wp_nav.set_desired_alt(current_target_alt + TOY_ALT_LARGE);
|
||||
}else{
|
||||
wp_nav.set_desired_alt(current_target_alt + TOY_ALT_SMALL);
|
||||
}
|
||||
|
||||
// Trigger downward alt change
|
||||
}else if(false == CH7_toy_flag && input < 1333) {
|
||||
CH7_toy_flag = true;
|
||||
// go down
|
||||
if(current_target_alt >= (400 + TOY_ALT_LARGE)) {
|
||||
wp_nav.set_desired_alt(current_target_alt - TOY_ALT_LARGE);
|
||||
}else if(current_target_alt >= TOY_ALT_SMALL) {
|
||||
wp_nav.set_desired_alt(current_target_alt - TOY_ALT_SMALL);
|
||||
}else if(current_target_alt < TOY_ALT_SMALL) {
|
||||
wp_nav.set_desired_alt(0);
|
||||
}
|
||||
|
||||
// clear flag
|
||||
}else if (CH7_toy_flag && ((input < 1666) && (input > 1333))) {
|
||||
CH7_toy_flag = false;
|
||||
}
|
||||
}
|
||||
|
||||
// called at 50 hz from all flight modes
|
||||
#if TOY_EDF == ENABLED
|
||||
void edf_toy()
|
||||
{
|
||||
// EDF control:
|
||||
g.rc_8.radio_out = 1000 + ((abs(g.rc_2.control_in) << 1) / 9);
|
||||
if(g.rc_8.radio_out < 1050)
|
||||
g.rc_8.radio_out = 1000;
|
||||
|
||||
// output throttle to EDF
|
||||
if(motors.armed()) {
|
||||
APM_RC.OutputCh(CH_8, g.rc_8.radio_out);
|
||||
}else{
|
||||
APM_RC.OutputCh(CH_8, 1000);
|
||||
}
|
||||
|
||||
// output Servo direction
|
||||
if(g.rc_2.control_in > 0) {
|
||||
APM_RC.OutputCh(CH_6, 1000); // 1000 : 2000
|
||||
}else{
|
||||
APM_RC.OutputCh(CH_6, 2000); // less than 1450
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// The function call for managing the flight mode Toy
|
||||
void roll_pitch_toy()
|
||||
static void
|
||||
get_roll_pitch_toy()
|
||||
{
|
||||
#if TOY_MIXER == TOY_LOOKUP_TABLE || TOY_MIXER == TOY_LINEAR_MIXER
|
||||
int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
|
||||
|
||||
if(g.rc_1.control_in != 0) { // roll
|
||||
get_acro_yaw(yaw_rate/2);
|
||||
ap.yaw_stopped = false;
|
||||
yaw_timer = 150;
|
||||
|
||||
}else if (!ap.yaw_stopped) {
|
||||
get_acro_yaw(0);
|
||||
yaw_timer--;
|
||||
|
||||
if((yaw_timer == 0) || (fabsf(omega.z) < 0.17f)) {
|
||||
ap.yaw_stopped = true;
|
||||
nav_yaw = ahrs.yaw_sensor;
|
||||
}
|
||||
}else{
|
||||
if(motors.armed() == false || g.rc_3.control_in == 0)
|
||||
nav_yaw = ahrs.yaw_sensor;
|
||||
|
||||
get_stabilize_yaw(nav_yaw);
|
||||
}
|
||||
#endif
|
||||
|
||||
// roll_rate is the outcome of the linear equation or lookup table
|
||||
// based on speed and Yaw rate
|
||||
int16_t roll_rate = 0;
|
||||
|
||||
#if TOY_MIXER == TOY_LOOKUP_TABLE
|
||||
uint8_t xx, yy;
|
||||
// Lookup value
|
||||
//xx = g_gps->ground_speed / 200;
|
||||
xx = abs(g.rc_2.control_in / 1000);
|
||||
yy = abs(yaw_rate / 500);
|
||||
|
||||
// constrain to lookup Array range
|
||||
xx = constrain_int16(xx, 0, 3);
|
||||
yy = constrain_int16(yy, 0, 8);
|
||||
|
||||
roll_rate = toy_lookup[yy * 4 + xx];
|
||||
|
||||
if(yaw_rate == 0) {
|
||||
roll_rate = 0;
|
||||
}else if(yaw_rate < 0) {
|
||||
roll_rate = -roll_rate;
|
||||
}
|
||||
|
||||
int16_t roll_limit = 4500 / g.toy_yaw_rate;
|
||||
roll_rate = constrain_int16(roll_rate, -roll_limit, roll_limit);
|
||||
|
||||
#elif TOY_MIXER == TOY_LINEAR_MIXER
|
||||
roll_rate = -((int32_t)g.rc_2.control_in * (yaw_rate/100)) /30;
|
||||
roll_rate = constrain_int32(roll_rate, -2000, 2000);
|
||||
|
||||
#elif TOY_MIXER == TOY_EXTERNAL_MIXER
|
||||
// JKR update to allow external roll/yaw mixing
|
||||
roll_rate = g.rc_1.control_in;
|
||||
#endif
|
||||
|
||||
#if TOY_EDF == ENABLED
|
||||
// Output the attitude
|
||||
//g.rc_1.servo_out = get_stabilize_roll(roll_rate);
|
||||
//g.rc_2.servo_out = get_stabilize_pitch(g.rc_6.control_in); // use CH6 to trim pitch
|
||||
get_stabilize_roll(roll_rate);
|
||||
get_stabilize_pitch(g.rc_6.control_in); // use CH6 to trim pitch
|
||||
|
||||
#else
|
||||
// Output the attitude
|
||||
//g.rc_1.servo_out = get_stabilize_roll(roll_rate);
|
||||
//g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in);
|
||||
get_stabilize_roll(roll_rate);
|
||||
get_stabilize_pitch(g.rc_2.control_in);
|
||||
#endif
|
||||
|
||||
// pass desired roll, pitch to stabilize attitude controllers
|
||||
get_stabilize_roll((float)control_roll * (1.0 - toy_gain));
|
||||
get_stabilize_pitch(control_pitch);
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user