diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index ecb46088ca..b05e5ad093 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -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(¤t_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; } } diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index 185055ef2f..3597557c88 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -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 diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde index 28a4ea6c4d..60ce3e0f98 100644 --- a/APMrover2/commands.pde +++ b/APMrover2/commands.pde @@ -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(); } diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index 322cbae69b..0deba92466 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -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; diff --git a/APMrover2/control_modes.pde b/APMrover2/control_modes.pde index efb6a07b9a..7b4cd37414 100644 --- a/APMrover2/control_modes.pde +++ b/APMrover2/control_modes.pde @@ -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); + } + } + } + } } diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 4cf121259c..ec378b0e62 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -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 diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde index c6a4bd3eb1..5d0ecc3209 100644 --- a/APMrover2/navigation.pde +++ b/APMrover2/navigation.pde @@ -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; } diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index 2f90b9ecc7..020874b49b 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -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 diff --git a/APMrover2/system.pde b/APMrover2/system.pde index eea0397ba4..83cc166740 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -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; + } +} diff --git a/APMrover2/test.pde b/APMrover2/test.pde index 82ffbac0fa..0835d39182 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -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++; }