ardupilot/ArduCopter/control_modes.pde

236 lines
5.4 KiB
Plaintext
Raw Normal View History

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void read_control_switch()
{
static bool switch_debouncer = false;
2012-01-03 14:23:05 -04:00
byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){
if(switch_debouncer){
oldSwitchPosition = switchPosition;
switch_debouncer = false;
set_mode(flight_modes[switchPosition]);
2011-12-03 21:54:57 -04:00
if(g.ch7_option != CH7_SIMPLE_MODE){
// set Simple mode using stored paramters from Mission planner
// rather than by the control switch
2011-09-17 01:33:01 -03:00
do_simple = (g.simple_modes & (1 << switchPosition));
2011-11-29 02:41:12 -04:00
}
}else{
switch_debouncer = true;
}
}
}
static byte readSwitch(void){
int pulsewidth = g.rc_5.radio_in; // default for Arducopter
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;
if (pulsewidth > 1490 && pulsewidth <= 1620) return 3;
if (pulsewidth > 1620 && pulsewidth <= 1749) return 4; // Software Manual
if (pulsewidth >= 1750) return 5; // Hardware Manual
return 0;
}
static void reset_control_switch()
{
oldSwitchPosition = -1;
read_control_switch();
}
#define CH_7_PWM_TRIGGER 1800
// read at 10 hz
// set this to your trainer switch
static void read_trim_switch()
{
#if CH7_OPTION == CH7_FLIP
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER && g.rc_3.control_in != 0){
init_flip();
2011-11-09 16:06:55 -04:00
}
#elif CH7_OPTION == CH7_SET_HOVER
2011-11-09 16:06:55 -04:00
// switch is engaged
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){
2011-11-09 16:06:55 -04:00
trim_flag = true;
2011-11-09 16:06:55 -04:00
}else{ // switch is disengaged
2011-11-09 16:06:55 -04:00
if(trim_flag){
2011-11-13 01:40:58 -04:00
trim_flag = false;
2011-11-09 16:06:55 -04:00
// set the throttle nominal
if(g.rc_3.control_in > 150){
g.throttle_cruise.set_and_save(g.rc_3.control_in);
//Serial.printf("tnom %d\n", g.throttle_cruise.get());
}
}
}
2011-11-26 17:21:16 -04:00
#else
// this is the normal operation set by the mission planner
if(g.ch7_option == CH7_SIMPLE_MODE){
do_simple = (g.rc_7.radio_in > CH_7_PWM_TRIGGER);
2011-11-26 17:21:16 -04:00
}else if (g.ch7_option == CH7_RTL){
if (trim_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER){
2011-11-09 16:06:55 -04:00
trim_flag = true;
2011-11-26 17:21:16 -04:00
set_mode(RTL);
}
2011-11-26 17:21:16 -04:00
if (trim_flag == true && g.rc_7.control_in < 800){
trim_flag = false;
if (control_mode == RTL || control_mode == LOITER){
reset_control_switch();
}
}
}else if (g.ch7_option == CH7_SAVE_WP){
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ // switch is engaged
2011-11-26 17:21:16 -04:00
trim_flag = true;
2011-11-26 17:21:16 -04:00
}else{ // switch is disengaged
2011-11-09 16:06:55 -04:00
if(trim_flag){
2011-11-13 01:40:58 -04:00
trim_flag = false;
2011-12-09 19:51:45 -04:00
if(control_mode == AUTO){
// reset the mission
CH7_wp_index = 0;
g.command_total.set_and_save(1);
set_mode(RTL);
return;
}
2011-12-09 19:51:45 -04:00
if(CH7_wp_index == 0){
// this is our first WP, let's save WP 1 as a takeoff
// increment index to WP index of 1 (home is stored at 0)
2011-12-09 19:51:45 -04:00
CH7_wp_index = 1;
Location temp = home;
2011-12-09 19:51:45 -04:00
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
temp.id = MAV_CMD_NAV_TAKEOFF;
temp.alt = current_loc.alt;
2011-12-09 19:51:45 -04:00
// save command:
// we use the current altitude to be the target for takeoff.
// only altitude will matter to the AP mission script for takeoff.
// If we are above the altitude, we will skip the command.
set_cmd_with_index(temp, CH7_wp_index);
2011-12-09 19:51:45 -04:00
}
2011-11-13 01:40:58 -04:00
// increment index
2011-11-09 16:06:55 -04:00
CH7_wp_index++;
2011-11-13 01:40:58 -04:00
// set the next_WP (home is stored at 0)
2011-11-13 01:40:58 -04:00
// max out at 100 since I think we need to stay under the EEPROM limit
CH7_wp_index = constrain(CH7_wp_index, 1, 100);
if(g.rc_3.control_in > 0){
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT
current_loc.id = MAV_CMD_NAV_WAYPOINT;
}else{
// set our location ID to 21, MAV_CMD_NAV_LAND
current_loc.id = MAV_CMD_NAV_LAND;
}
2011-11-13 01:40:58 -04:00
// save command
2011-11-20 03:24:14 -04:00
set_cmd_with_index(current_loc, CH7_wp_index);
2011-11-13 01:40:58 -04:00
copter_leds_nav_blink = 10; // Cause the CopterLEDs to blink twice to indicate saved waypoint
// 0 = home
// 1 = takeoff
// 2 = WP 2
// 3 = command total
2011-11-09 16:06:55 -04:00
}
}
}else if (g.ch7_option == CH7_AUTO_TRIM){
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){
auto_level_counter = 10;
}
2011-11-26 17:21:16 -04:00
}
#endif
2011-09-21 20:24:09 -03:00
}
static void auto_trim()
{
if(auto_level_counter > 0){
//g.rc_1.dead_zone = 60; // 60 = .6 degrees
//g.rc_2.dead_zone = 60;
auto_level_counter--;
trim_accel();
led_mode = AUTO_TRIM_LEDS;
2011-12-09 19:51:45 -04:00
do_simple = false;
if(auto_level_counter == 1){
//g.rc_1.dead_zone = 0; // 60 = .6 degrees
//g.rc_2.dead_zone = 0;
led_mode = NORMAL_LEDS;
clear_leds();
imu.save();
2011-12-09 19:51:45 -04:00
reset_control_switch();
//Serial.println("Done");
auto_level_counter = 0;
}
}
}
2011-12-09 19:51:45 -04:00
/*
How this works:
Level Example:
A_off: -14.00, -20.59, -30.80
Right roll Example:
A_off: -6.73, 89.05, -46.02
Left Roll Example:
A_off: -18.11, -160.31, -56.42
Pitch Forward:
A_off: -127.00, -22.16, -50.09
Pitch Back:
A_off: 201.95, -24.00, -88.56
*/
static void trim_accel()
{
reset_stability_I();
2011-12-10 19:05:28 -04:00
float trim_roll = (float)g.rc_1.control_in / 30000.0;
float trim_pitch = (float)g.rc_2.control_in / 30000.0;
2011-12-09 19:51:45 -04:00
trim_roll = constrain(trim_roll, -1.5, 1.5);
trim_pitch = constrain(trim_pitch, -1.5, 1.5);
if(g.rc_1.control_in > 200){ // Roll Right
2011-12-10 19:05:28 -04:00
imu.ay(imu.ay() - trim_roll);
2011-12-09 19:51:45 -04:00
}else if (g.rc_1.control_in < -200){
2011-12-10 19:05:28 -04:00
imu.ay(imu.ay() - trim_roll);
}
2011-12-09 19:51:45 -04:00
if(g.rc_2.control_in > 200){ // Pitch Back
imu.ax(imu.ax() + trim_pitch);
}else if (g.rc_2.control_in < -200){
imu.ax(imu.ax() + trim_pitch);
}
/*
2011-12-10 19:05:28 -04:00
Serial.printf_P(PSTR("r:%1.2f %1.2f \t| p:%1.2f %1.2f\n"),
trim_roll,
(float)imu.ay(),
2011-12-10 19:05:28 -04:00
trim_pitch,
(float)imu.ax());
//*/
}