Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: more cleanups

Browse Source 
get rid of flight_mode_strings and cleanup unused code

switch to using ahrs for have_position
This commit is contained in:
Andrew Tridgell 2012-11-28 22:44:03 +11:00
parent e0109357f0
commit ebafad7a6b
10 changed files with 99 additions and 162 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
APMrover2/APMrover2.pde
View File

@ -316,36 +316,12 @@ static bool usb_connected;
static const char *comma = ",";
static const char* flight_mode_strings[] = {
"Manual",
"",
"Learning",
"",
"",
"",
"",
"",
"",
"",
"Auto",
"RTL",
"",
"",
"",
"",
"",
"",
"",
"",
"",
""};
/* Radio values
Channel assignments
1 Ailerons (rudder if no ailerons)
2 Elevator
1 Steering
2 ---
3 Throttle
4 Rudder (if we have ailerons)
4 ---
5 Aux5
6 Aux6
7 Aux7
@ -416,6 +392,11 @@ static int32_t gps_base_alt;
// Constants
const float radius_of_earth = 6378100; // meters
const float gravity = 9.81; // meters/ sec^2
// true if we have a position estimate from AHRS
static bool have_position;
// This is the currently calculated direction to fly.
// deg * 100 : 0 to 360
static int32_t nav_bearing;
@ -789,16 +770,7 @@ cliSerial->println(tempaccel.z, DEC);
//------------------------------------------------
case 1:
medium_loopCounter++;
if(g_gps->new_data){
g_gps->new_data = false;
// calculate the plane's desired bearing
// -------------------------------------
navigate();
}
navigate();
break;
// command processing
@ -922,9 +894,9 @@ static void update_GPS(void)
g_gps->update();
update_GPS_light();
if (g_gps->new_data && g_gps->fix) {
// for performance
// ---------------
have_position = ahrs.get_position(&current_loc);
if (g_gps->new_data && g_gps->status() == GPS::GPS_OK) {
gps_fix_count++;
if(ground_start_count > 1){
@ -947,11 +919,6 @@ static void update_GPS(void)
ground_start_count = 0;
}
}
current_loc.lng = g_gps->longitude; // Lon * 10**7
current_loc.lat = g_gps->latitude; // Lat * 10**7
current_loc.alt = max((g_gps->altitude - home.alt),0);
ground_speed = g_gps->ground_speed;
}
}

2
APMrover2/Log.pde
View File

@ -517,7 +517,7 @@ static void Log_Read_Attitude()
static void Log_Read_Mode()
{
cliSerial->printf_P(PSTR("MOD: "));
cliSerial->println(flight_mode_strings[DataFlash.ReadByte()]);
print_flight_mode(DataFlash.ReadByte());
}
// Read a GPS packet

43
APMrover2/commands.pde
View File

@ -155,40 +155,21 @@ static void set_guided_WP(void)
// -------------------------------
void init_home()
{
if (!have_position) {
// we need position information
return;
}
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
// block until we get a good fix
// -----------------------------
while (!g_gps->new_data || !g_gps->fix) {
g_gps->update();
}
home.id = MAV_CMD_NAV_WAYPOINT;
#if HIL_MODE != HIL_MODE_ATTITUDE
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
gps_base_alt = max(g_gps->altitude, 0);
home.alt = g_gps->altitude;;
// Home is always 0
#else
// struct Location temp = get_cmd_with_index(0); // JLN update - for HIL test only get the home param stored in the FPL
// if (temp.alt > 0) {
// home.lng = temp.lng; // Lon * 10**7
// home.lat = temp.lat; // Lat * 10**7
// } else {
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
// }
gps_base_alt = g_gps->altitude;; // get the stored home altitude as the base ref for AGL calculation.
home.alt = g_gps->altitude;;
#endif
gps_base_alt = max(g_gps->altitude, 0);
home.alt = g_gps->altitude;
home_is_set = true;
//gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
// Save Home to EEPROM - Command 0
// -------------------
set_cmd_with_index(home, 0);
@ -204,9 +185,9 @@ void init_home()
static void restart_nav()
{
reset_I();
prev_WP = current_loc;
nav_command_ID = NO_COMMAND;
nav_command_index = 0;
process_next_command();
reset_I();
prev_WP = current_loc;
nav_command_ID = NO_COMMAND;
nav_command_index = 0;
process_next_command();
}

2
APMrover2/commands_logic.pde
View File

@ -351,7 +351,7 @@ static void do_change_speed()
static void do_set_home()
{
if(next_nonnav_command.p1 == 1 && g_gps->status() == GPS::GPS_OK) {
if(next_nonnav_command.p1 == 1 && have_position) {
init_home();
} else {
home.id = MAV_CMD_NAV_WAYPOINT;

95
APMrover2/control_modes.pde
View File

@ -55,69 +55,62 @@ static void reset_control_switch()
// set this to your trainer switch
static void read_trim_switch()
{
if (g.ch7_option == CH7_SAVE_WP){ // set to 1
if (g.ch7_option == CH7_SAVE_WP) { // set to 1
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ // switch is engaged
trim_flag = true;
}else{ // switch is disengaged
if(trim_flag){
} else { // switch is disengaged
if(trim_flag) {
trim_flag = false;
if(control_mode == MANUAL){ // if SW7 is ON in MANUAL = Erase the Flight Plan
// reset the mission
if (control_mode == MANUAL) {
// if SW7 is ON in MANUAL = Erase the Flight Plan
CH7_wp_index = 0;
g.command_total.set_and_save(CH7_wp_index);
g.command_total = 0;
g.command_index =0;
nav_command_index = 0;
if(g.channel_roll.control_in > 3000) // if roll is full right store the current location as home
ground_start_count = 5;
#if X_PLANE == ENABLED
cliSerial->printf_P(PSTR("*** RESET the FPL\n"));
#endif
CH7_wp_index = 1;
g.command_total = 0;
g.command_index =0;
nav_command_index = 0;
if(g.channel_roll.control_in > 3000) // if roll is full right store the current location as home
ground_start_count = 5;
CH7_wp_index = 1;
return;
} else if (control_mode == LEARNING) { // if SW7 is ON in LEARNING = record the Wp
// set the next_WP (home is stored at 0)
// max out at 100 since I think we need to stay under the EEPROM limit
CH7_wp_index = constrain(CH7_wp_index, 1, 100);
} else if (control_mode == LEARNING) {
// if SW7 is ON in LEARNING = record the Wp
// set the next_WP (home is stored at 0)
CH7_wp_index = constrain(CH7_wp_index, 1, MAX_WAYPOINTS);
current_loc.id = MAV_CMD_NAV_WAYPOINT;
current_loc.id = MAV_CMD_NAV_WAYPOINT;
// store the index
g.command_total.set_and_save(CH7_wp_index);
g.command_total = CH7_wp_index;
g.command_index = CH7_wp_index;
nav_command_index = 0;
// store the index
g.command_total.set_and_save(CH7_wp_index);
g.command_total = CH7_wp_index;
g.command_index = CH7_wp_index;
nav_command_index = 0;
// save command
set_cmd_with_index(current_loc, CH7_wp_index);
#if X_PLANE == ENABLED
cliSerial->printf_P(PSTR("*** Store WP #%d\n"),CH7_wp_index);
#endif
// save command
set_cmd_with_index(current_loc, CH7_wp_index);
// increment index
CH7_wp_index++;
// increment index
CH7_wp_index++;
} else if (control_mode == AUTO) { // if SW7 is ON in AUTO = set to RTL
set_mode(RTL);
}
}
}
}
else if (g.ch7_option == CH7_RTL){ // set to 6
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ // switch is engaged
trim_flag = true;
}else{ // switch is disengaged
if(trim_flag){
trim_flag = false;
if (control_mode == LEARNING) {
set_mode(RTL);
}
}
} else if (control_mode == AUTO) {
// if SW7 is ON in AUTO = set to RTL
set_mode(RTL);
}
}
}
}
} else if (g.ch7_option == CH7_RTL) {
// set to 6
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
// switch is engaged
trim_flag = true;
} else { // switch is disengaged
if (trim_flag) {
trim_flag = false;
if (control_mode == LEARNING) {
set_mode(RTL);
}
}
}
}
}

25
APMrover2/defines.h
View File

@ -35,32 +35,7 @@
// CH 7 control
#define CH7_DO_NOTHING 0
#define CH7_SAVE_WP 1
#define CH7_LEO 2
#define CH7_THERMAL 3
#define CH7_SARSEC 4
#define CH7_SARGRID 5
#define CH7_RTL 6
#define CH7_TUNING 7
// CH_7 Tuning
// -----------
#define TUN_NONE 0
// Attitude
#define TUN_STABILIZE_KP 1
#define TUN_STABILIZE_KI 2
#define TUN_STABILIZE_KD 3
#define TUN_YAW_KP 4
#define TUN_YAW_KI 5
#define TUN_YAW_KD 6
#define TUN_STABROLL_KP 7
#define TUN_STABROLL_KI 8
#define TUN_STABROLL_KD 9
#define TUN_STABPITCH_KP 10
#define TUN_STABPITCH_KI 11
#define TUN_STABPITCH_KD 12
#define PITOT_SOURCE_ADC 1
#define PITOT_SOURCE_ANALOG_PIN 2
#define T6 1000000
#define T7 10000000

4
APMrover2/navigation.pde
View File

@ -7,9 +7,7 @@ static void navigate()
{
// do not navigate with corrupt data
// ---------------------------------
if (g_gps->fix == 0)
{
g_gps->new_data = false;
if (!have_position) {
return;
}

2
APMrover2/setup.pde
View File

@ -552,7 +552,7 @@ static void
print_switch(byte p, byte m)
{
cliSerial->printf_P(PSTR("Pos %d: "),p);
cliSerial->println(flight_mode_strings[m]);
print_flight_mode(m);
}
static void

25
APMrover2/system.pde
View File

@ -453,7 +453,7 @@ static void startup_INS_ground(bool force_accel_level)
// when MANUAL_LEVEL is set to 1 we don't do accelerometer
// levelling on each boot, and instead rely on the user to do
// it once via the ground station
ins.init_accel(mavlink_delay, flash_leds);
ins.init_accel(mavlink_delay, flash_leds);
}
ahrs.set_fly_forward(true);
ahrs.reset();
@ -565,3 +565,26 @@ uint16_t board_voltage(void)
return vcc.read_vcc();
}
#endif
static void
print_flight_mode(uint8_t mode)
{
switch (mode) {
case MANUAL:
cliSerial->println_P(PSTR("Manual"));
break;
case LEARNING:
cliSerial->println_P(PSTR("Learning"));
break;
case AUTO:
cliSerial->println_P(PSTR("AUTO"));
break;
case RTL:
cliSerial->println_P(PSTR("RTL"));
break;
default:
cliSerial->println_P(PSTR("---"));
break;
}
}

6
APMrover2/test.pde
View File

@ -226,15 +226,15 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
fail_test++;
}
if(oldSwitchPosition != readSwitch()){
if (oldSwitchPosition != readSwitch()){
cliSerial->printf_P(PSTR("CONTROL MODE CHANGED: "));
cliSerial->println(flight_mode_strings[readSwitch()]);
print_flight_mode(readSwitch());
fail_test++;
}
if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){
cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in);
cliSerial->println(flight_mode_strings[readSwitch()]);
print_flight_mode(readSwitch());
fail_test++;
}