From 51ada5987cccf378913e3fac004ae653e7fd33e1 Mon Sep 17 00:00:00 2001 From: "mich146@hotmail.com" Date: Sat, 16 Apr 2011 01:00:50 +0000 Subject: [PATCH] Fix ACM Logging was logging PM out of control. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1883 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/system.pde | 1012 ++++++++++++++++++------------------- 1 file changed, 505 insertions(+), 507 deletions(-) diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index ff2abd3b71..00f31077f0 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -1,508 +1,506 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/***************************************************************************** -The init_ardupilot function processes everything we need for an in - air restart - We will determine later if we are actually on the ground and process a - ground start in that case. - -*****************************************************************************/ - -// Functions called from the top-level menu -extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde -extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde -extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp - -// This is the help function -// PSTR is an AVR macro to read strings from flash memory -// printf_P is a version of print_f that reads from flash memory -static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) -{ - Serial.printf_P(PSTR("Commands:\n" - " logs log readback/setup mode\n" - " setup setup mode\n" - " test test mode\n" - "\n" - "Move the slide switch and reset to FLY.\n" - "\n")); - return(0); -} - -// Command/function table for the top-level menu. -const struct Menu::command main_menu_commands[] PROGMEM = { -// command function called -// ======= =============== - {"logs", process_logs}, - {"setup", setup_mode}, - {"test", test_mode}, - {"help", main_menu_help} -}; - -// Create the top-level menu object. -MENU(main_menu, "ArduPilotMega", main_menu_commands); - -void init_ardupilot() -{ - byte last_log_num; - int last_log_start; - int last_log_end; - - // Console serial port - // - // The console port buffers are defined to be sufficiently large to support - // the console's use as a logging device, optionally as the GPS port when - // GPS_PROTOCOL_IMU is selected, and as the telemetry port. - // - // XXX This could be optimised to reduce the buffer sizes in the cases - // where they are not otherwise required. - // - Serial.begin(SERIAL0_BAUD, 128, 128); - - // GPS serial port. - // - // Not used if the IMU/X-Plane GPS is in use. - // - // XXX currently the EM406 (SiRF receiver) is nominally configured - // at 57600, however it's not been supported to date. We should - // probably standardise on 38400. - // - // XXX the 128 byte receive buffer may be too small for NMEA, depending - // on the message set configured. - // -#if GPS_PROTOCOL != GPS_PROTOCOL_IMU - Serial1.begin(38400, 128, 16); -#endif - - // Telemetry port. - // - // Not used if telemetry is going to the console. - // - // XXX for unidirectional protocols, we could (should) minimize - // the receive buffer, and the transmit buffer could also be - // shrunk for protocols that don't send large messages. - // - Serial3.begin(SERIAL3_BAUD, 128, 128); - - Serial.printf_P(PSTR("\n\nInit ArduCopterMega" - "\n\nFree RAM: %lu\n"), - freeRAM()); - - - // - // Check the EEPROM format version before loading any parameters from EEPROM. - // - report_version(); - - if (!g.format_version.load() || - g.format_version != Parameters::k_format_version) { - //Serial.printf_P(PSTR("\n\nForcing complete parameter reset...")); - - /*Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" - "\n\nForcing complete parameter reset..."), - g.format_version.get(), - Parameters::k_format_version); - */ - - // erase all parameters - AP_Var::erase_all(); - - // save the new format version - g.format_version.set_and_save(Parameters::k_format_version); - - Serial.printf_P(PSTR("Please Run Setup...\n")); - while (true) { - delay(100); - if(motor_light){ - digitalWrite(A_LED_PIN, HIGH); - digitalWrite(B_LED_PIN, HIGH); - digitalWrite(C_LED_PIN, HIGH); - }else{ - digitalWrite(A_LED_PIN, LOW); - digitalWrite(B_LED_PIN, LOW); - digitalWrite(C_LED_PIN, LOW); - } - motor_light = !motor_light; - } - - }else{ - unsigned long before = micros(); - // Load all auto-loaded EEPROM variables - AP_Var::load_all(); - - // Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); - // Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use()); - } - -#ifdef RADIO_OVERRIDE_DEFAULTS - { - int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS; - APM_RC.setHIL(rc_override); - } -#endif - - init_rc_in(); // sets up rc channels from radio - init_rc_out(); // sets up the timer libs - init_camera(); -#if HIL_MODE != HIL_MODE_ATTITUDE - adc.Init(); // APM ADC library initialization - barometer.Init(); // APM Abs Pressure sensor initialization -#endif - DataFlash.Init(); // DataFlash log initialization - - // Do GPS init - //g_gps = &GPS; - g_gps = &g_gps_driver; - g_gps->init(); // GPS Initialization - - // init the GCS - #if GCS_PORT == 3 - gcs.init(&Serial3); - #else - gcs.init(&Serial); - #endif - - - if (g.log_bitmask & MASK_LOG_MODE) - Log_Write_Mode(control_mode); - - if(g.compass_enabled) - init_compass(); - -#if HIL_MODE != HIL_MODE_ATTITUDE - if(g.sonar_enabled){ - sonar.init(SONAR_PIN, &adc); - } -#endif - - pinMode(C_LED_PIN, OUTPUT); // GPS status LED - pinMode(A_LED_PIN, OUTPUT); // GPS status LED - pinMode(B_LED_PIN, OUTPUT); // GPS status LED - pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode - pinMode(PUSHBUTTON_PIN, INPUT); // unused - DDRL |= B00000100; // Set Port L, pin 2 to output for the relay - - // If the switch is in 'menu' mode, run the main menu. - // - // Since we can't be sure that the setup or test mode won't leave - // the system in an odd state, we don't let the user exit the top - // menu; they must reset in order to fly. - // - if (digitalRead(SLIDE_SWITCH_PIN) == 0) { - digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED - Serial.printf_P(PSTR("\n" - "Entering interactive setup mode...\n" - "\n" - "Type 'help' to list commands, 'exit' to leave a submenu.\n" - "Visit the 'setup' menu for first-time configuration.\n\n")); - for (;;) { - //Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n")); - main_menu.run(); - } - } - - if(g.log_bitmask > 0){ - // TODO - Here we will check on the length of the last log - // We don't want to create a bunch of little logs due to powering on and off - last_log_num = get_num_logs(); - start_new_log(last_log_num); - } - - // read in the flight switches - update_servo_switches(); - - // read in the flight switches - //update_servo_switches(); - - //imu.init_gyro(IMU::WARM_START); - - startup_ground(); - - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Startup(TYPE_GROUNDSTART_MSG); - - if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) { - default_log_bitmask(); - } - - // set the correct flight mode - // --------------------------- - reset_control_switch(); -} - -//******************************************************************************** -//This function does all the calibrations, etc. that we need during a ground start -//******************************************************************************** -void startup_ground(void) -{ - gcs.send_text_P(SEVERITY_LOW,PSTR(" GROUND START")); - - #if(GROUND_START_DELAY > 0) - //gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay")); - delay(GROUND_START_DELAY * 1000); - #endif - - setup_show(NULL,NULL); - - // setup up PID value max - init_pids(); - - // Output waypoints for confirmation - // -------------------------------- - for(int i = 1; i < g.waypoint_total + 1; i++) { - gcs.send_message(MSG_COMMAND_LIST, i); - } - -#if HIL_MODE != HIL_MODE_ATTITUDE - // Warm up and read Gyro offsets - // ----------------------------- - imu.init_gyro(); - report_imu(); -#endif - - // read the radio to set trims - // --------------------------- - //trim_radio(); - - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Startup(TYPE_GROUNDSTART_MSG); - - -#if HIL_MODE != HIL_MODE_ATTITUDE - // read Baro pressure at ground - //----------------------------- - init_barometer(); -#endif - - // initialize commands - // ------------------- - init_commands(); - - byte counter = 4; - GPS_enabled = false; - - // Read in the GPS - for (byte counter = 0; ; counter++) { - g_gps->update(); - if (g_gps->status() != 0){ - GPS_enabled = true; - break; - } - - if (counter >= 2) { - GPS_enabled = false; - break; - } - } - report_gps(); - SendDebug("\nReady to FLY "); - gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); -} - -void set_mode(byte mode) -{ - if(control_mode == mode){ - // don't switch modes if we are already in the correct mode. - return; - } - - control_mode = mode; - control_mode = constrain(control_mode, 0, NUM_MODES - 1); - - // XXX temporary - //if (g.log_bitmask & MASK_LOG_MODE) - Log_Write_Mode(control_mode); - - // used to stop fly_aways - if(g.rc_1.control_in == 0){ - // we are on the ground is this is true - // disarm motors temp - motor_auto_safe = false; - } - - - // clear our tracking behaviors - yaw_tracking = TRACK_NONE; - - //send_text_P(SEVERITY_LOW,PSTR("control mode")); - //Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode); - switch(control_mode) - { - case ACRO: - break; - - case STABILIZE: - do_loiter_at_location(); - break; - - case ALT_HOLD: - init_throttle_cruise(); - do_loiter_at_location(); - break; - - case AUTO: - init_throttle_cruise(); - init_auto(); - break; - - case SIMPLE: - break; - - case LOITER: - init_throttle_cruise(); - do_loiter_at_location(); - break; - - case RTL: - init_throttle_cruise(); - do_RTL(); - break; - - default: - break; - } - - // output control mode to the ground station - gcs.send_message(MSG_MODE_CHANGE); -} - -void set_failsafe(boolean mode) -{ - // only act on changes - // ------------------- - if(failsafe != mode){ - - // store the value so we don't trip the gate twice - // ----------------------------------------------- - failsafe = mode; - - if (failsafe == false){ - // We've regained radio contact - // ---------------------------- - failsafe_off_event(); - - }else{ - // We've lost radio contact - // ------------------------ - failsafe_on_event(); - - } - } -} - -void update_GPS_light(void) -{ - // GPS LED on if we have a fix or Blink GPS LED if we are receiving data - // --------------------------------------------------------------------- - switch (g_gps->status()){ - - case(2): - digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. - break; - - case(1): - if (g_gps->valid_read == true){ - GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock - if (GPS_light){ - digitalWrite(C_LED_PIN, LOW); - }else{ - digitalWrite(C_LED_PIN, HIGH); - } - g_gps->valid_read = false; - } - break; - - default: - digitalWrite(C_LED_PIN, LOW); - break; - } -} - -void update_motor_light(void) -{ - if(motor_armed == true){ - motor_light = !motor_light; - - // blink - if(motor_light){ - digitalWrite(A_LED_PIN, HIGH); - }else{ - digitalWrite(A_LED_PIN, LOW); - } - }else{ - if(!motor_light){ - motor_light = true; - digitalWrite(A_LED_PIN, HIGH); - } - } -} - -void update_timer_light(bool light) -{ - if(light == true){ - digitalWrite(B_LED_PIN, HIGH); - }else{ - digitalWrite(B_LED_PIN, LOW); - } -} - - -void resetPerfData(void) { - /* - mainLoop_count = 0; - G_Dt_max = 0; - gyro_sat_count = 0; - adc_constraints = 0; - renorm_sqrt_count = 0; - renorm_blowup_count = 0; - gps_fix_count = 0; - perf_mon_timer = millis(); - */ -} - -void -init_compass() -{ - dcm.set_compass(&compass); - bool junkbool = compass.init(); - compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft - Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference -} - - -/* This function gets the current value of the heap and stack pointers. -* The stack pointer starts at the top of RAM and grows downwards. The heap pointer -* starts just above the static variables etc. and grows upwards. SP should always -* be larger than HP or you'll be in big trouble! The smaller the gap, the more -* careful you need to be. Julian Gall 6 - Feb - 2009. -*/ -unsigned long freeRAM() { - uint8_t * heapptr, * stackptr; - stackptr = (uint8_t *)malloc(4); // use stackptr temporarily - heapptr = stackptr; // save value of heap pointer - free(stackptr); // free up the memory again (sets stackptr to 0) - stackptr = (uint8_t *)(SP); // save value of stack pointer - return stackptr - heapptr; -} - -void -init_simple_bearing() -{ - initial_simple_bearing = dcm.yaw_sensor; - //if(simple_bearing_is_set == false){ - //if(g.rc_3.control_in == 0){ - // we are on the ground - // initial_simple_bearing = dcm.yaw_sensor; - // simple_bearing_is_set = true; - //} - //} -} - -void -init_throttle_cruise() -{ - if(set_throttle_cruise_flag == false){ - if(g.rc_3.control_in > 200){ - //set_throttle_cruise_flag = true; - g.throttle_cruise.set_and_save(g.rc_3.control_in); - //Serial.printf("throttle_cruise: %d\n", g.throttle_cruise.get()); - } - } +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/***************************************************************************** +The init_ardupilot function processes everything we need for an in - air restart + We will determine later if we are actually on the ground and process a + ground start in that case. + +*****************************************************************************/ + +// Functions called from the top-level menu +extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde +extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde +extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp + +// This is the help function +// PSTR is an AVR macro to read strings from flash memory +// printf_P is a version of print_f that reads from flash memory +static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv) +{ + Serial.printf_P(PSTR("Commands:\n" + " logs log readback/setup mode\n" + " setup setup mode\n" + " test test mode\n" + "\n" + "Move the slide switch and reset to FLY.\n" + "\n")); + return(0); +} + +// Command/function table for the top-level menu. +const struct Menu::command main_menu_commands[] PROGMEM = { +// command function called +// ======= =============== + {"logs", process_logs}, + {"setup", setup_mode}, + {"test", test_mode}, + {"help", main_menu_help} +}; + +// Create the top-level menu object. +MENU(main_menu, "ArduPilotMega", main_menu_commands); + +void init_ardupilot() +{ + byte last_log_num; + int last_log_start; + int last_log_end; + + // Console serial port + // + // The console port buffers are defined to be sufficiently large to support + // the console's use as a logging device, optionally as the GPS port when + // GPS_PROTOCOL_IMU is selected, and as the telemetry port. + // + // XXX This could be optimised to reduce the buffer sizes in the cases + // where they are not otherwise required. + // + Serial.begin(SERIAL0_BAUD, 128, 128); + + // GPS serial port. + // + // Not used if the IMU/X-Plane GPS is in use. + // + // XXX currently the EM406 (SiRF receiver) is nominally configured + // at 57600, however it's not been supported to date. We should + // probably standardise on 38400. + // + // XXX the 128 byte receive buffer may be too small for NMEA, depending + // on the message set configured. + // +#if GPS_PROTOCOL != GPS_PROTOCOL_IMU + Serial1.begin(38400, 128, 16); +#endif + + // Telemetry port. + // + // Not used if telemetry is going to the console. + // + // XXX for unidirectional protocols, we could (should) minimize + // the receive buffer, and the transmit buffer could also be + // shrunk for protocols that don't send large messages. + // + Serial3.begin(SERIAL3_BAUD, 128, 128); + + Serial.printf_P(PSTR("\n\nInit ArduCopterMega" + "\n\nFree RAM: %lu\n"), + freeRAM()); + + + // + // Check the EEPROM format version before loading any parameters from EEPROM. + // + report_version(); + + if (!g.format_version.load() || + g.format_version != Parameters::k_format_version) { + //Serial.printf_P(PSTR("\n\nForcing complete parameter reset...")); + + /*Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" + "\n\nForcing complete parameter reset..."), + g.format_version.get(), + Parameters::k_format_version); + */ + + // erase all parameters + AP_Var::erase_all(); + + // save the new format version + g.format_version.set_and_save(Parameters::k_format_version); + + Serial.printf_P(PSTR("Please Run Setup...\n")); + while (true) { + delay(100); + if(motor_light){ + digitalWrite(A_LED_PIN, HIGH); + digitalWrite(B_LED_PIN, HIGH); + digitalWrite(C_LED_PIN, HIGH); + }else{ + digitalWrite(A_LED_PIN, LOW); + digitalWrite(B_LED_PIN, LOW); + digitalWrite(C_LED_PIN, LOW); + } + motor_light = !motor_light; + } + + }else{ + unsigned long before = micros(); + // Load all auto-loaded EEPROM variables + AP_Var::load_all(); + + // Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); + // Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use()); + } + +#ifdef RADIO_OVERRIDE_DEFAULTS + { + int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS; + APM_RC.setHIL(rc_override); + } +#endif + + init_rc_in(); // sets up rc channels from radio + init_rc_out(); // sets up the timer libs + init_camera(); +#if HIL_MODE != HIL_MODE_ATTITUDE + adc.Init(); // APM ADC library initialization + barometer.Init(); // APM Abs Pressure sensor initialization +#endif + DataFlash.Init(); // DataFlash log initialization + + // Do GPS init + //g_gps = &GPS; + g_gps = &g_gps_driver; + g_gps->init(); // GPS Initialization + + // init the GCS + #if GCS_PORT == 3 + gcs.init(&Serial3); + #else + gcs.init(&Serial); + #endif + + + if (g.log_bitmask & MASK_LOG_MODE) + Log_Write_Mode(control_mode); + + if(g.compass_enabled) + init_compass(); + +#if HIL_MODE != HIL_MODE_ATTITUDE + if(g.sonar_enabled){ + sonar.init(SONAR_PIN, &adc); + } +#endif + + pinMode(C_LED_PIN, OUTPUT); // GPS status LED + pinMode(A_LED_PIN, OUTPUT); // GPS status LED + pinMode(B_LED_PIN, OUTPUT); // GPS status LED + pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode + pinMode(PUSHBUTTON_PIN, INPUT); // unused + DDRL |= B00000100; // Set Port L, pin 2 to output for the relay + + // If the switch is in 'menu' mode, run the main menu. + // + // Since we can't be sure that the setup or test mode won't leave + // the system in an odd state, we don't let the user exit the top + // menu; they must reset in order to fly. + // + if (digitalRead(SLIDE_SWITCH_PIN) == 0) { + digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED + Serial.printf_P(PSTR("\n" + "Entering interactive setup mode...\n" + "\n" + "Type 'help' to list commands, 'exit' to leave a submenu.\n" + "Visit the 'setup' menu for first-time configuration.\n\n")); + for (;;) { + //Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n")); + main_menu.run(); + } + } + + if(g.log_bitmask > 0){ + // TODO - Here we will check on the length of the last log + // We don't want to create a bunch of little logs due to powering on and off + last_log_num = get_num_logs(); + start_new_log(last_log_num); + } + + // read in the flight switches + update_servo_switches(); + + // read in the flight switches + //update_servo_switches(); + + //imu.init_gyro(IMU::WARM_START); + + startup_ground(); + + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Startup(TYPE_GROUNDSTART_MSG); + + if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) { + default_log_bitmask(); + } + + // set the correct flight mode + // --------------------------- + reset_control_switch(); +} + +//******************************************************************************** +//This function does all the calibrations, etc. that we need during a ground start +//******************************************************************************** +void startup_ground(void) +{ + gcs.send_text_P(SEVERITY_LOW,PSTR(" GROUND START")); + + #if(GROUND_START_DELAY > 0) + //gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay")); + delay(GROUND_START_DELAY * 1000); + #endif + + setup_show(NULL,NULL); + + // setup up PID value max + init_pids(); + + // Output waypoints for confirmation + // -------------------------------- + for(int i = 1; i < g.waypoint_total + 1; i++) { + gcs.send_message(MSG_COMMAND_LIST, i); + } + +#if HIL_MODE != HIL_MODE_ATTITUDE + // Warm up and read Gyro offsets + // ----------------------------- + imu.init_gyro(); + report_imu(); +#endif + + // read the radio to set trims + // --------------------------- + //trim_radio(); + + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Startup(TYPE_GROUNDSTART_MSG); + + +#if HIL_MODE != HIL_MODE_ATTITUDE + // read Baro pressure at ground + //----------------------------- + init_barometer(); +#endif + + // initialize commands + // ------------------- + init_commands(); + + byte counter = 4; + GPS_enabled = false; + + // Read in the GPS + for (byte counter = 0; ; counter++) { + g_gps->update(); + if (g_gps->status() != 0){ + GPS_enabled = true; + break; + } + + if (counter >= 2) { + GPS_enabled = false; + break; + } + } + report_gps(); + SendDebug("\nReady to FLY "); + gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY.")); +} + +void set_mode(byte mode) +{ + if(control_mode == mode){ + // don't switch modes if we are already in the correct mode. + return; + } + + control_mode = mode; + control_mode = constrain(control_mode, 0, NUM_MODES - 1); + + // XXX temporary + //if (g.log_bitmask & MASK_LOG_MODE) + Log_Write_Mode(control_mode); + + // used to stop fly_aways + if(g.rc_1.control_in == 0){ + // we are on the ground is this is true + // disarm motors temp + motor_auto_safe = false; + } + + + // clear our tracking behaviors + yaw_tracking = TRACK_NONE; + + //send_text_P(SEVERITY_LOW,PSTR("control mode")); + //Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode); + switch(control_mode) + { + case ACRO: + break; + + case STABILIZE: + do_loiter_at_location(); + break; + + case ALT_HOLD: + init_throttle_cruise(); + do_loiter_at_location(); + break; + + case AUTO: + init_throttle_cruise(); + init_auto(); + break; + + case SIMPLE: + break; + + case LOITER: + init_throttle_cruise(); + do_loiter_at_location(); + break; + + case RTL: + init_throttle_cruise(); + do_RTL(); + break; + + default: + break; + } + + // output control mode to the ground station + gcs.send_message(MSG_MODE_CHANGE); +} + +void set_failsafe(boolean mode) +{ + // only act on changes + // ------------------- + if(failsafe != mode){ + + // store the value so we don't trip the gate twice + // ----------------------------------------------- + failsafe = mode; + + if (failsafe == false){ + // We've regained radio contact + // ---------------------------- + failsafe_off_event(); + + }else{ + // We've lost radio contact + // ------------------------ + failsafe_on_event(); + + } + } +} + +void update_GPS_light(void) +{ + // GPS LED on if we have a fix or Blink GPS LED if we are receiving data + // --------------------------------------------------------------------- + switch (g_gps->status()){ + + case(2): + digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. + break; + + case(1): + if (g_gps->valid_read == true){ + GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock + if (GPS_light){ + digitalWrite(C_LED_PIN, LOW); + }else{ + digitalWrite(C_LED_PIN, HIGH); + } + g_gps->valid_read = false; + } + break; + + default: + digitalWrite(C_LED_PIN, LOW); + break; + } +} + +void update_motor_light(void) +{ + if(motor_armed == true){ + motor_light = !motor_light; + + // blink + if(motor_light){ + digitalWrite(A_LED_PIN, HIGH); + }else{ + digitalWrite(A_LED_PIN, LOW); + } + }else{ + if(!motor_light){ + motor_light = true; + digitalWrite(A_LED_PIN, HIGH); + } + } +} + +void update_timer_light(bool light) +{ + if(light == true){ + digitalWrite(B_LED_PIN, HIGH); + }else{ + digitalWrite(B_LED_PIN, LOW); + } +} + + +void resetPerfData(void) { + mainLoop_count = 0; + G_Dt_max = 0; + gyro_sat_count = 0; + adc_constraints = 0; + renorm_sqrt_count = 0; + renorm_blowup_count = 0; + gps_fix_count = 0; + perf_mon_timer = millis(); +} + +void +init_compass() +{ + dcm.set_compass(&compass); + bool junkbool = compass.init(); + compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft + Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference +} + + +/* This function gets the current value of the heap and stack pointers. +* The stack pointer starts at the top of RAM and grows downwards. The heap pointer +* starts just above the static variables etc. and grows upwards. SP should always +* be larger than HP or you'll be in big trouble! The smaller the gap, the more +* careful you need to be. Julian Gall 6 - Feb - 2009. +*/ +unsigned long freeRAM() { + uint8_t * heapptr, * stackptr; + stackptr = (uint8_t *)malloc(4); // use stackptr temporarily + heapptr = stackptr; // save value of heap pointer + free(stackptr); // free up the memory again (sets stackptr to 0) + stackptr = (uint8_t *)(SP); // save value of stack pointer + return stackptr - heapptr; +} + +void +init_simple_bearing() +{ + initial_simple_bearing = dcm.yaw_sensor; + //if(simple_bearing_is_set == false){ + //if(g.rc_3.control_in == 0){ + // we are on the ground + // initial_simple_bearing = dcm.yaw_sensor; + // simple_bearing_is_set = true; + //} + //} +} + +void +init_throttle_cruise() +{ + if(set_throttle_cruise_flag == false){ + if(g.rc_3.control_in > 200){ + //set_throttle_cruise_flag = true; + g.throttle_cruise.set_and_save(g.rc_3.control_in); + //Serial.printf("throttle_cruise: %d\n", g.throttle_cruise.get()); + } + } } \ No newline at end of file