From acb7d78a790124db06b29fa1938b3e48dd69fb69 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sat, 19 Feb 2011 03:59:58 +0000 Subject: [PATCH] updated to AP_Var git-svn-id: https://arducopter.googlecode.com/svn/trunk@1680 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 9 +- ArduCopterMega/EEPROM.pde | 167 +++++++--------- ArduCopterMega/Parameters.h | 18 +- ArduCopterMega/commands.pde | 2 +- ArduCopterMega/commands_process.pde | 264 ++++++++++++------------- ArduCopterMega/control_modes.pde | 8 +- ArduCopterMega/defines.h | 58 +++++- ArduCopterMega/events.pde | 42 ++-- ArduCopterMega/motors.pde | 2 +- ArduCopterMega/read_commands.pde | 10 +- ArduCopterMega/sensors.pde | 4 +- ArduCopterMega/setup.pde | 290 +++++++++++++++------------- ArduCopterMega/system.pde | 12 +- ArduCopterMega/test.pde | 12 +- 14 files changed, 459 insertions(+), 439 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index ca34f2a8b5..d7957b5243 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -1,7 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /* -ArduCopterMega Version 0.1 Experimental +ArduCopterMega Version 0.1.3 Experimental Authors: Jason Short Based on code and ideas from the Arducopter team: Jose Julio, Randy Mackay, Jani Hirvinen Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier @@ -216,8 +216,7 @@ byte command_may_index; // current command memory location byte command_must_ID; // current command ID byte command_may_ID; // current command ID -float altitude_gain; // in nav -float distance_gain; // in nav +//float altitude_gain; // in nav float cos_roll_x; float sin_roll_y; @@ -281,11 +280,9 @@ int milliamp_hours; // Barometer Sensor variables // -------------------------- -//int baro_offset; // used to correct drift of absolute pressue sensor unsigned long abs_pressure; -unsigned long abs_pressure_ground; +unsigned long ground_pressure; int ground_temperature; -//int temp_unfilt; byte altitude_sensor = BARO; // used to know whic sensor is active, BARO or SONAR diff --git a/ArduCopterMega/EEPROM.pde b/ArduCopterMega/EEPROM.pde index 52687a7887..070395ba33 100644 --- a/ArduCopterMega/EEPROM.pde +++ b/ArduCopterMega/EEPROM.pde @@ -5,7 +5,7 @@ // Macro functions // --------------- -void read_EEPROM_startup(void) +/*void read_EEPROM_startup(void) { read_EEPROM_PID(); read_EEPROM_frame(); @@ -13,27 +13,17 @@ void read_EEPROM_startup(void) read_EEPROM_logs(); read_EEPROM_flight_modes(); read_EEPROM_waypoint_info(); - imu.load_gyro_eeprom(); - imu.load_accel_eeprom(); + //imu.load_gyro_eeprom(); + //imu.load_accel_eeprom(); read_EEPROM_alt_RTL(); read_EEPROM_current(); read_EEPROM_nav(); // magnatometer read_EEPROM_compass(); - read_EEPROM_compass_declination(); + //read_EEPROM_compass_declination(); read_EEPROM_compass_offset(); } - -void read_EEPROM_airstart_critical(void) -{ - // Read the home location - //----------------------- - home = get_wp_with_index(0); - - // Read pressure sensor values - //---------------------------- - read_EEPROM_pressure(); -} +*/ void save_EEPROM_groundstart(void) { @@ -56,56 +46,45 @@ void save_EEPROM_groundstart(void) void save_EEPROM_alt_RTL(void) { - eeprom_write_dword((uint32_t *)EE_ALT_HOLD_HOME, alt_to_hold); + g.RTL_altitude.save(); + } void read_EEPROM_alt_RTL(void) { - alt_to_hold = eeprom_read_dword((const uint32_t *) EE_ALT_HOLD_HOME); + g.RTL_altitude.load(); } /********************************************************************************/ void read_EEPROM_waypoint_info(void) { - g.waypoint_total = eeprom_read_byte((uint8_t *) EE_WP_TOTAL); - wp_radius = eeprom_read_byte((uint8_t *) EE_WP_RADIUS); - loiter_radius = eeprom_read_byte((uint8_t *) EE_LOITER_RADIUS); + g.waypoint_total.load(); + g.waypoint_radius.load(); + g.loiter_radius.load(); } void save_EEPROM_waypoint_info(void) { - eeprom_write_byte((uint8_t *) EE_WP_TOTAL, g.waypoint_total); - eeprom_write_byte((uint8_t *) EE_WP_RADIUS, wp_radius); - eeprom_write_byte((uint8_t *) EE_LOITER_RADIUS, loiter_radius); + g.waypoint_total.save(); + g.waypoint_radius.save(); + g.loiter_radius.save(); } /********************************************************************************/ void read_EEPROM_nav(void) { - // for nav estimation - distance_gain = read_EE_compressed_float(EE_DISTANCE_GAIN, 4); - altitude_gain = read_EE_compressed_float(EE_ALTITUDE_GAIN, 4); - - // stored as degree * 100 - g.crosstrack_gain = read_EE_compressed_float(EE_XTRACK_GAIN, 4); - g.crosstrack_entry_angle = eeprom_read_word((uint16_t *) EE_XTRACK_ANGLE) * 100; - pitch_max = eeprom_read_word((uint16_t *) EE_PITCH_MAX); // stored as degress * 100 + g.crosstrack_gain.load(); + g.crosstrack_entry_angle.load(); + g.pitch_max.load(); } void save_EEPROM_nav(void) { - // for nav estimation - write_EE_compressed_float(altitude_gain, EE_ALTITUDE_GAIN, 4); - write_EE_compressed_float(distance_gain, EE_DISTANCE_GAIN, 4); - write_EE_compressed_float(crosstrack_gain, EE_XTRACK_GAIN, 4); - - // stored as degrees - eeprom_write_word((uint16_t *) EE_XTRACK_ANGLE, g.crosstrack_entry_angle / 100); - - // stored as degrees - eeprom_write_word((uint16_t *) EE_PITCH_MAX, pitch_max); + g.crosstrack_gain.save(); + g.crosstrack_entry_angle.save(); + g.pitch_max.save(); } /********************************************************************************/ @@ -126,10 +105,10 @@ void read_EEPROM_PID(void) g.pid_sonar_throttle.load_gains(); // roll pitch - stabilize_dampener = read_EE_compressed_float(EE_STAB_DAMPENER, 4); + g.stabilize_dampener.load(); // yaw - hold_yaw_dampener = read_EE_compressed_float(EE_HOLD_YAW_DAMPENER, 4); + g.hold_yaw_dampener.load(); init_pids(); } @@ -149,61 +128,47 @@ void save_EEPROM_PID(void) g.pid_sonar_throttle.save_gains(); // roll pitch - write_EE_compressed_float(stabilize_dampener, EE_STAB_DAMPENER, 4); - + g.stabilize_dampener.save(); // yaw - write_EE_compressed_float(hold_yaw_dampener, EE_HOLD_YAW_DAMPENER, 4); + g.hold_yaw_dampener.save(); } /********************************************************************************/ void save_EEPROM_frame(void) { - eeprom_write_byte((uint8_t *)EE_FRAME, frame_type); + g.frame_type.save(); } void read_EEPROM_frame(void) { - frame_type = eeprom_read_byte((uint8_t *) EE_FRAME); + g.frame_type.load(); } /********************************************************************************/ -void save_EEPROM_g.(void) +void save_EEPROM_throttle_cruise (void) { - eeprom_write_word((uint16_t *)EE_THROTTLE_CRUISE, g.); + g.throttle_cruise.save(); } -void read_EEPROM_g.(void) +void read_EEPROM_throttle_cruise(void) { - g.throttle_cruise = eeprom_read_word((uint16_t *) EE_THROTTLE_CRUISE); -} - -/********************************************************************************/ - -void save_EEPROM_mag_declination(void) -{ - write_EE_compressed_float(mag_declination, EE_MAG_DECLINATION, 1); -} - -void read_EEPROM_compass_declination(void) -{ - mag_declination = read_EE_compressed_float(EE_MAG_DECLINATION, 1); + g.throttle_cruise.load(); } /********************************************************************************/ void save_EEPROM_current(void) { - eeprom_write_byte((uint8_t *) EE_CURRENT_SENSOR, current_enabled); - eeprom_write_word((uint16_t *) EE_CURRENT_MAH, milliamp_hours); - + g.current_enabled.save(); + g.milliamp_hours.save(); } void read_EEPROM_current(void) { - current_enabled = eeprom_read_byte((uint8_t *) EE_CURRENT_SENSOR); - milliamp_hours = eeprom_read_word((uint16_t *) EE_CURRENT_MAH); + g.current_enabled.load(); + g.milliamp_hours.load(); } /********************************************************************************/ @@ -226,40 +191,42 @@ void read_EEPROM_compass_offset(void) void read_EEPROM_compass(void) { - compass_enabled = eeprom_read_byte((uint8_t *) EE_COMPASS); + g.compass_enabled.load(); } void save_EEPROM_mag(void) { - eeprom_write_byte((uint8_t *) EE_COMPASS, compass_enabled); + g.compass_enabled.save(); } /********************************************************************************/ void save_command_index(void) { - eeprom_write_byte((uint8_t *) EE_WP_INDEX, command_must_index); + g.command_must_index.save(); } void read_command_index(void) { - g.waypoint_index = command_must_index = eeprom_read_byte((uint8_t *) EE_WP_INDEX); + g.command_must_index.load(); } /********************************************************************************/ void save_EEPROM_pressure(void) { - eeprom_write_dword((uint32_t *)EE_ABS_PRESS_GND, abs_pressure_ground); - eeprom_write_word((uint16_t *)EE_GND_TEMP, ground_temperature); + g.ground_pressure.save(); + g.ground_temperature.save(); + } void read_EEPROM_pressure(void) { - abs_pressure_ground = eeprom_read_dword((uint32_t *) EE_ABS_PRESS_GND); - // Better than zero for an air start value - abs_pressure = abs_pressure_ground; - ground_temperature = eeprom_read_word((uint16_t *) EE_GND_TEMP); + g.ground_pressure.load(); + g.ground_temperature.load(); + + // to prime the filter + abs_pressure = g.ground_pressure; } /********************************************************************************/ @@ -292,52 +259,51 @@ void save_EEPROM_radio(void) // configs are the basics void read_EEPROM_throttle(void) { - throttle_min = eeprom_read_word((uint16_t *) EE_THROTTLE_MIN); - throttle_max = eeprom_read_word((uint16_t *) EE_THROTTLE_MAX); - read_EEPROM_g.(); - throttle_failsafe_enabled = eeprom_read_byte((byte *) EE_THROTTLE_FAILSAFE); - throttle_failsafe_action = eeprom_read_byte((byte *) EE_THROTTLE_FAILSAFE_ACTION); - throttle_failsafe_value = eeprom_read_word((uint16_t *) EE_THROTTLE_FS_VALUE); + g.throttle_min.load(); + g.throttle_max.load(); + g.throttle_cruise.load(); + g.throttle_failsafe_enabled.load(); + g.throttle_failsafe_action.load(); + g.throttle_failsafe_value.load(); } void save_EEPROM_throttle(void) { - eeprom_write_word((uint16_t *) EE_THROTTLE_MIN, throttle_min); - eeprom_write_word((uint16_t *) EE_THROTTLE_MAX, throttle_max); - save_EEPROM_g.(); - eeprom_write_byte((byte *) EE_THROTTLE_FAILSAFE, throttle_failsafe_enabled); - eeprom_write_byte((byte *) EE_THROTTLE_FAILSAFE_ACTION,throttle_failsafe_action); - eeprom_write_word((uint16_t *) EE_THROTTLE_FS_VALUE, throttle_failsafe_value); + g.throttle_min.load(); + g.throttle_max.load(); + g.throttle_cruise.save(); + g.throttle_failsafe_enabled.load(); + g.throttle_failsafe_action.load(); + g.throttle_failsafe_value.load(); } /********************************************************************************/ void read_EEPROM_logs(void) { - g.log_bitmask = eeprom_read_word((uint16_t *) EE_LOG_BITMASK); + g.log_bitmask.load(); } void save_EEPROM_logs(void) { - eeprom_write_word((uint16_t *) EE_LOG_BITMASK, log_bitmask); + g.log_bitmask.save(); } /********************************************************************************/ void read_EEPROM_flight_modes(void) { - // Read Radio min/max settings - eeprom_read_block((void*)&flight_modes, (const void*)EE_FLIGHT_MODES, sizeof(flight_modes)); + g.flight_modes.load(); } void save_EEPROM_flight_modes(void) { - // Save Radio min/max settings - eeprom_write_block((const void *)&flight_modes, (void *)EE_FLIGHT_MODES, sizeof(flight_modes)); + g.flight_modes.save(); + } /********************************************************************************/ - +/* float read_EE_float(int address) { @@ -362,9 +328,9 @@ void write_EE_float(float value, int address) for (int i = 0; i < 4; i++) eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]); } - +*/ /********************************************************************************/ - +/* float read_EE_compressed_float(int address, byte places) { @@ -380,3 +346,4 @@ void write_EE_compressed_float(float value, int address, byte places) int temp = value * scale; eeprom_write_word((uint16_t *) address, temp); } +*/ \ No newline at end of file diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index d71116d3a7..62f2ef69a4 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -55,11 +55,10 @@ public: // k_param_IMU_calibration = 140, k_param_ground_temperature, - k_param_ground_altitude, k_param_ground_pressure, k_param_current, + k_param_milliamp_hours, k_param_compass, - k_param_mag_declination, // // 160: Navigation parameters @@ -97,6 +96,7 @@ public: k_param_waypoint_mode = 220, k_param_waypoint_total, k_param_waypoint_index, + k_param_command_must_index, k_param_waypoint_radius, k_param_loiter_radius, @@ -138,6 +138,7 @@ public: AP_Int8 waypoint_mode; AP_Int8 waypoint_total; AP_Int8 waypoint_index; + AP_Int8 command_must_index; AP_Int8 waypoint_radius; AP_Int8 loiter_radius; @@ -168,14 +169,13 @@ public: // AP_Int16 log_bitmask; AP_Int16 ground_temperature; - AP_Int16 ground_altitude; AP_Int16 ground_pressure; AP_Int16 RTL_altitude; AP_Int8 frame_type; AP_Int8 current_enabled; + AP_Int16 milliamp_hours; AP_Int8 compass_enabled; - AP_Float mag_declination; // RC channels @@ -218,21 +218,22 @@ public: frame_type (FRAME_CONFIG, k_param_frame_type, PSTR("FRAME_CONFIG")), current_enabled (DISABLED, k_param_current, PSTR("CURRENT_ENABLE")), + milliamp_hours (2100, k_param_milliamp_hours, PSTR("MAH")), compass_enabled (DISABLED, k_param_compass, PSTR("COMPASS_ENABLE")), - mag_declination (0, k_param_mag_declination, PSTR("MAG_DEC")), waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")), waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), + command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")), waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("LOITER_RADIUS")), - throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), - throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), + throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), + throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), throttle_failsafe_enabled (THROTTLE_FAILSAFE, k_param_throttle_failsafe_enabled, PSTR("THR_FAILSAFE")), throttle_failsafe_action (THROTTLE_FAILSAFE_ACTION, k_param_throttle_failsafe_action, PSTR("THR_FS_ACTION")), throttle_failsafe_value (THROTTLE_FS_VALUE, k_param_throttle_failsafe_value, PSTR("THR_FS_VALUE")), - throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), + throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), flight_mode_channel (FLIGHT_MODE_CHANNEL, k_param_flight_mode_channel, PSTR("FLIGHT_MODE_CH")), flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")), @@ -241,7 +242,6 @@ public: log_bitmask (0, k_param_log_bitmask, PSTR("LOG_BITMASK")), ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), - ground_altitude (0, k_param_ground_altitude, PSTR("GND_ALT_CM")), ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 2001df0e04..a6aa150982 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -155,7 +155,7 @@ void set_next_WP(struct Location *wp) { Serial.printf_P(PSTR("set_next_WP, wp_index %d\n"), (int)g.waypoint_index); - send_message(MSG_COMMAND, g.waypoint_index); + send_message(MSG_COMMAND_LIST, g.waypoint_index); // copy the current WP into the OldWP slot // --------------------------------------- diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 228349617c..376c153d1f 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -8,12 +8,12 @@ void update_commands(void) if((home_is_set == false) || (control_mode != AUTO)){ return; // don't do commands } - + if(control_mode == AUTO){ load_next_command(); process_next_command(); } - + //verify_must(); //verify_may(); } @@ -26,32 +26,32 @@ void load_next_command() if(next_command.id == CMD_BLANK){ next_command = get_wp_with_index(g.waypoint_index + 1); if(next_command.id != CMD_BLANK){ - //Serial.print("MSG fetch found new cmd from list at index: "); - //Serial.println((g.waypoint_index + 1),DEC); - //Serial.print("MSG cmd id: "); - //Serial.println(next_command.id,DEC); + //SendDebug("MSG fetch found new cmd from list at index "); + //SendDebug((g.waypoint_index + 1),DEC); + //SendDebug(" with cmd id "); + //SendDebugln(next_command.id,DEC); } } - + // If the preload failed, return or just Loiter // generate a dynamic command for RTL // -------------------------------------------- if(next_command.id == CMD_BLANK){ // we are out of commands! - //send_message(SEVERITY_LOW,"out of commands!"); - //Serial.print("MSG out of commands, wp_index: "); - //Serial.println(g.waypoint_index,DEC); + gcs.send_text(SEVERITY_LOW,"out of commands!"); + //SendDebug("MSG out of commands, g.waypoint_index: "); + //SendDebugln(g.waypoint_index,DEC); + - switch (control_mode){ case LAND: // don't get a new command break; - + default: next_command = get_LOITER_home_wp(); - //Serial.print("MSG Preload RTL cmd id: "); - //Serial.println(next_command.id,DEC); + //SendDebug("MSG Preload RTL cmd id: "); + //SendDebugln(next_command.id,DEC); break; } } @@ -62,25 +62,25 @@ void process_next_command() // these are waypoint/Must commands // --------------------------------- if (command_must_index == 0){ // no current command loaded - if (next_command.id >= 0x10 && next_command.id <= 0x1F ){ + if (next_command.id < MAV_CMD_NAV_LAST ){ increment_WP_index(); - save_command_index(); // to Recover from in air Restart + //save_command_index(); // to Recover from in air Restart command_must_index = g.waypoint_index; - - //Serial.print("MSG new command_must_id "); - //Serial.print(next_command.id,DEC); - //Serial.print(" index:"); - //Serial.println(command_must_index,DEC); - if (g.log_bitmask & MASK_LOG_CMD) + + //SendDebug("MSG new command_must_id "); + //SendDebug(next_command.id,DEC); + //SendDebug(" index:"); + //SendDebugln(command_must_index,DEC); + if (g.log_bitmask & MASK_LOG_CMD) Log_Write_Cmd(g.waypoint_index, &next_command); process_must(); } } - + // these are May commands // ---------------------- if (command_may_index == 0){ - if (next_command.id >= 0x20 && next_command.id <= 0x2F ){ + if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ increment_WP_index();// this command is from the command list in EEPROM command_may_index = g.waypoint_index; //Serial.print("new command_may_index "); @@ -90,50 +90,50 @@ void process_next_command() process_may(); } } - + // these are do it now commands // --------------------------- - if (next_command.id >= 0x30){ - increment_WP_index();// this command is from the command list in EEPROM + if (next_command.id > MAV_CMD_CONDITION_LAST){ + increment_WP_index();// this command is from the command list in EEPROM if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); + Log_Write_Cmd(g.waypoint_index, &next_command); process_now(); } } - /* These functions implement the waypoint commands. */ void process_must() { - //Serial.print("process must index: "); - //Serial.println(command_must_index,DEC); + //SendDebug("process must index: "); + //SendDebugln(command_must_index,DEC); + + gcs.send_text(SEVERITY_LOW,"New cmd: "); + gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); - send_message(SEVERITY_LOW,"New cmd: "); - send_message(MSG_COMMAND, g.waypoint_index); - // clear May indexes command_may_index = 0; command_may_ID = 0; command_must_ID = next_command.id; - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = 0; - - // reset navigation integrators - // ------------------------- - reset_I(); - // loads the waypoint into Next_WP struct // -------------------------------------- set_next_WP(&next_command); - + + // invalidate command so a new one is loaded + // ----------------------------------------- + next_command.id = 0; + + // reset navigation integrators + // ------------------------- + reset_I(); + switch(command_must_ID){ - - case CMD_TAKEOFF: // TAKEOFF! + + case MAV_CMD_NAV_TAKEOFF: // TAKEOFF! + // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 takeoff_altitude = (int)next_command.alt; next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs @@ -142,10 +142,10 @@ void process_must() //set_next_WP(&next_WP); break; - case CMD_WAYPOINT: // Navigate to Waypoint + case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint break; - case CMD_R_WAYPOINT: // Navigate to Waypoint + case MAV_CMD_NAV_R_WAYPOINT: // Navigate to Waypoint next_command.lat += home.lat; // offset from home location next_command.lng += home.lng; // offset from home location @@ -153,7 +153,7 @@ void process_must() set_next_WP(&next_command); break; - case CMD_LAND: // LAND to Waypoint + case MAV_CMD_NAV_LAND: // LAND to Waypoint velocity_land = 1000; next_WP.lat = current_loc.lat; next_WP.lng = current_loc.lng; @@ -161,53 +161,53 @@ void process_must() land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction break; - case CMD_ALTITUDE: // Loiter indefinitely + /* + case MAV_CMD_ALTITUDE: // next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs break; + */ - case CMD_LOITER: // Loiter indefinitely + case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely break; - case CMD_LOITER_N_TURNS: // Loiter N Times + case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times break; - case CMD_LOITER_TIME: + case MAV_CMD_NAV_LOITER_TIME: break; - case CMD_RTL: + case MAV_CMD_NAV_RETURN_TO_LAUNCH: return_to_launch(); break; - } } void process_may() { //Serial.print("process_may cmd# "); - //Serial.println(g.waypoint_index,DEC); + //Serial.println(g.waypoint_index,DEC); command_may_ID = next_command.id; // invalidate command so a new one is loaded // ----------------------------------------- next_command.id = 0; - - send_message(SEVERITY_LOW,"New cmd: "); - send_message(MSG_COMMAND, g.waypoint_index); - + + gcs.send_text(SEVERITY_LOW," New may command loaded:"); + gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); // do the command // -------------- switch(command_may_ID){ - - case CMD_DELAY: // Navigate to Waypoint + + case MAV_CMD_CONDITION_DELAY: // Navigate to Waypoint delay_start = millis(); delay_timeout = next_command.lat; break; - case CMD_LAND_OPTIONS: // Land this puppy - break; + //case MAV_CMD_NAV_LAND_OPTIONS: // Land this puppy + // break; - case CMD_ANGLE: + case MAV_CMD_CONDITION_ANGLE: // p1: bearing // alt: speed // lat: direction (-1,1), @@ -281,38 +281,16 @@ void process_now() // invalidate command so a new one is loaded // ----------------------------------------- next_command.id = 0; - - send_message(SEVERITY_LOW, "New cmd: "); - send_message(MSG_COMMAND, g.waypoint_index); - + + gcs.send_text(SEVERITY_LOW, " New now command loaded: "); + gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); + // do the command // -------------- switch(id){ - - case CMD_RESET_INDEX: - init_commands(); - break; - - case CMD_GETVAR_INDEX: - // - break; - - case CMD_SENDVAR_INDEX: - // - break; - - case CMD_TELEMETRY: - // - break; - - //case CMD_AIRSPEED_CRUISE: - //airspeed_cruise = next_command.p1 * 100; - // todo save to EEPROM - //break; - + /* case CMD_THROTTLE_CRUISE: g.throttle_cruise = next_command.p1; - // todo save to EEPROM break; case CMD_RESET_HOME: @@ -356,29 +334,33 @@ void process_now() // radio_min[next_command.p1] = next_command.alt; // save_EEPROM_radio_minmax(); // break; - - case CMD_REPEAT: + */ + + case MAV_CMD_DO_SET_HOME: + init_home(); + break; + + case MAV_CMD_DO_REPEAT_SERVO: new_event(&next_command); break; - case CMD_SERVO: + case MAV_CMD_DO_SET_SERVO: + //Serial.print("MAV_CMD_DO_SET_SERVO "); + //Serial.print(next_command.p1,DEC); + //Serial.print(" PWM: "); + //Serial.println(next_command.alt,DEC); APM_RC.OutputCh(next_command.p1, next_command.alt); break; - - case CMD_INDEX: - command_must_index = 0; - command_may_index = 0; - g.waypoint_index = next_command.p1 - 1; - break; - case CMD_RELAY: - if(next_command.p1 = 0){ - relay_A(); + case MAV_CMD_DO_SET_RELAY: + if (next_command.p1 == 0) { + relay_on(); + } else if (next_command.p1 == 1) { + relay_off(); }else{ - relay_B(); + relay_toggle(); } break; - } } @@ -386,24 +368,24 @@ void process_now() // --------------- void verify_must() { - float power; switch(command_must_ID) { - case CMD_ALTITUDE: + /*case MAV_CMD_ALTITUDE: if (abs(next_WP.alt - current_loc.alt) < 100){ command_must_index = 0; } break; + */ - case CMD_TAKEOFF: // Takeoff! + case MAV_CMD_NAV_TAKEOFF: // Takeoff! if (current_loc.alt > (next_WP.alt -100)){ command_must_index = 0; takeoff_complete = true; } break; - case CMD_LAND: + case MAV_CMD_NAV_LAND: // 10 - 9 = 1 velocity_land = ((old_alt - current_loc.alt) *.05) + (velocity_land * .95); old_alt = current_loc.alt; @@ -411,12 +393,19 @@ void verify_must() land_complete = true; command_must_index = 0; } + update_crosstrack(); + break; - - case CMD_WAYPOINT: // reach a waypoint - if ((wp_distance > 0) && (wp_distance <= waypoint_radius)) { - Serial.print("MSG REACHED_WAYPOINT #"); - Serial.println(command_must_index,DEC); + + case MAV_CMD_NAV_WAYPOINT: // reach a waypoint + update_crosstrack(); + if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { + //SendDebug("MSG REACHED_WAYPOINT #"); + //SendDebugln(command_must_index,DEC); + char message[50]; + sprintf(message,"Reached Waypoint #%i",command_must_index); + gcs.send_text(SEVERITY_LOW,message); + // clear the command queue; command_must_index = 0; } @@ -428,23 +417,35 @@ void verify_must() } break; - case CMD_LOITER_N_TURNS: // LOITER N times + case MAV_CMD_NAV_LOITER_TURNS: // LOITER N times break; - - case CMD_LOITER_TIME: // loiter N milliseconds + + case MAV_CMD_NAV_LOITER_TIME: // loiter N milliseconds break; - - case CMD_RTL: - if (wp_distance <= waypoint_radius) { - //Serial.println("REACHED_HOME"); + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + if (wp_distance <= g.waypoint_radius) { + gcs.send_text(SEVERITY_LOW,"Reached home"); command_must_index = 0; } break; - case CMD_LOITER: // Just plain LOITER - break; + //case MAV_CMD_NAV_LOITER_UNLIM: // Just plain LOITER + // break; - case CMD_ANGLE: + + default: + gcs.send_text(SEVERITY_HIGH," No current Must commands"); + break; + } +} + +void verify_may() +{ + float power; + switch(command_may_ID) { + + case MAV_CMD_CONDITION_ANGLE: if((millis() - command_yaw_start_time) > command_yaw_time){ command_must_index = 0; nav_yaw = command_yaw_end; @@ -456,23 +457,14 @@ void verify_must() } break; - default: - send_message(SEVERITY_HIGH,"No current Must commands"); - break; - } -} - -void verify_may() -{ - switch(command_may_ID) { - case CMD_DELAY: + case MAV_CMD_CONDITION_DELAY: if ((millis() - delay_start) > delay_timeout){ command_may_index = 0; delay_timeout = 0; } - case CMD_LAND_OPTIONS: - break; + //case CMD_LAND_OPTIONS: + // break; } } diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index e4bcb52569..7b1b20eb23 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -72,14 +72,14 @@ void read_trim_switch() if(trim_flag){ // switch was just released if((millis() - trim_timer) > 2000){ - // not being used + imu.save(); } else { // set the throttle nominal if(g.rc_3.control_in > 50){ - g.throttle_cruise = g.rc_3.control_in; - Serial.printf("tnom %d\n", g.); - save_EEPROM_g.(); + g.throttle_cruise.set(g.rc_3.control_in); + Serial.printf("tnom %d\n", g.throttle_cruise.get()); + save_EEPROM_throttle_cruise(); } } trim_flag = false; diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 06926d6a1d..1f915b08c3 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -62,7 +62,8 @@ #define SONAR 0 #define BARO 1 -#define WP_START_BYTE 0x130 // where in memory home WP is stored + all other WP +// parameters get the first 1KiB of EEPROM, remainder is for waypoints +#define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP #define WP_SIZE 14 // GCS enumeration @@ -156,19 +157,60 @@ #define MSG_PRESSURE 0x04 #define MSG_STATUS_TEXT 0x05 #define MSG_PERF_REPORT 0x06 -#define MSG_COMMAND 0x22 +#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed +#define MSG_VERSION_REQUEST 0x08 +#define MSG_VERSION 0x09 +#define MSG_EXTENDED_STATUS 0x0a +#define MSG_CPU_LOAD 0x0b + +#define MSG_COMMAND_REQUEST 0x20 +#define MSG_COMMAND_UPLOAD 0x21 +#define MSG_COMMAND_LIST 0x22 +#define MSG_COMMAND_MODE_CHANGE 0x23 +#define MSG_CURRENT_WAYPOINT 0x24 + +#define MSG_VALUE_REQUEST 0x30 +#define MSG_VALUE_SET 0x31 #define MSG_VALUE 0x32 + +#define MSG_PID_REQUEST 0x40 +#define MSG_PID_SET 0x41 #define MSG_PID 0x42 -#define MSG_TRIMS 0x50 -#define MSG_MINS 0x51 -#define MSG_MAXS 0x52 -#define MSG_IMU_OUT 0x53 +#define MSG_VFR_HUD 0x4a + +#define MSG_TRIM_STARTUP 0x50 +#define MSG_TRIM_MIN 0x51 +#define MSG_TRIM_MAX 0x52 +#define MSG_RADIO_OUT 0x53 +#define MSG_RADIO_IN 0x54 + +#define MSG_RAW_IMU 0x60 +#define MSG_GPS_STATUS 0x61 +#define MSG_GPS_RAW 0x62 + +#define MSG_SERVO_OUT 0x70 + +#define MSG_PIN_REQUEST 0x80 +#define MSG_PIN_SET 0x81 + +#define MSG_DATAFLASH_REQUEST 0x90 +#define MSG_DATAFLASH_SET 0x91 + +#define MSG_EEPROM_REQUEST 0xa0 +#define MSG_EEPROM_SET 0xa1 + +#define MSG_POSITION_CORRECT 0xb0 +#define MSG_ATTITUDE_CORRECT 0xb1 +#define MSG_POSITION_SET 0xb2 +#define MSG_ATTITUDE_SET 0xb3 +#define MSG_LOCAL_LOCATION 0xb4 #define SEVERITY_LOW 1 #define SEVERITY_MEDIUM 2 #define SEVERITY_HIGH 3 #define SEVERITY_CRITICAL 4 + // Logging parameters #define LOG_ATTITUDE_MSG 0x01 #define LOG_GPS_MSG 0x02 @@ -269,8 +311,8 @@ #define EE_XTRACK_GAIN 0x30 #define EE_XTRACK_ANGLE 0x32 #define EE_PITCH_MAX 0x34 -#define EE_DISTANCE_GAIN 0x36 -#define EE_ALTITUDE_GAIN 0x38 +//#define EE_DISTANCE_GAIN 0x36 +//#define EE_ALTITUDE_GAIN 0x38 #define EE_GAIN_1 0x40 // all gains stored from here #define EE_GAIN_2 0x48 // all gains stored from here diff --git a/ArduCopterMega/events.pde b/ArduCopterMega/events.pde index d92a904053..1b54b74e0e 100644 --- a/ArduCopterMega/events.pde +++ b/ArduCopterMega/events.pde @@ -1,3 +1,5 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + /* This event will be called when the failsafe changes boolean failsafe reflects the current state @@ -18,9 +20,9 @@ void failsafe_event() void low_battery_event(void) { - send_message(SEVERITY_HIGH,"Low Battery!"); + gcs.send_text(SEVERITY_HIGH,"Low Battery!"); set_mode(RTL); - throttle_cruise = THROTTLE_CRUISE; + g.throttle_cruise.set(THROTTLE_CRUISE); } @@ -42,12 +44,12 @@ undo_event void new_event(struct Location *cmd) { - Serial.print("New Event Loaded "); - Serial.println(cmd->p1,DEC); + SendDebug("New Event Loaded "); + SendDebugln(cmd->p1,DEC); if(cmd->p1 == STOP_REPEAT){ - Serial.println("STOP repeat "); + SendDebugln("STOP repeat "); event_id = NO_REPEAT; event_timer = -1; undo_event = 0; @@ -116,25 +118,26 @@ void perform_event() case RELAY_TOGGLE: event_undo_value = PORTL & B00000100 ? 1:0; - if(event_undo_value == 1){ - relay_A(); - }else{ - relay_B(); - } - Serial.print("toggle relay "); - Serial.println(PORTL,BIN); + relay_toggle(); + SendDebug("toggle relay "); + SendDebugln(PORTL,BIN); undo_event = 2; break; } } -void relay_A() +void relay_on() { PORTL |= B00000100; } -void relay_B() +void relay_off() +{ + PORTL &= ~B00000100; +} + +void relay_toggle() { PORTL ^= B00000100; } @@ -184,14 +187,9 @@ void perform_event_undo() break; case RELAY_TOGGLE: - - if(event_undo_value == 1){ - relay_A(); - }else{ - relay_B(); - } - Serial.print("untoggle relay "); - Serial.println(PORTL,BIN); + relay_toggle(); + SendDebug("untoggle relay "); + SendDebugln(PORTL,BIN); break; } } diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 3d052d18fb..9ccb7aeb1c 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -274,6 +274,6 @@ set_servos_4() reset_I(); // Initialize yaw command to actual yaw when throttle is down... - g.rc_4.control_in = ToDeg(yaw); + g.rc_4.control_in = ToDeg(dcm.yaw); } } diff --git a/ArduCopterMega/read_commands.pde b/ArduCopterMega/read_commands.pde index 3d9ceae888..384e1d8769 100644 --- a/ArduCopterMega/read_commands.pde +++ b/ArduCopterMega/read_commands.pde @@ -79,30 +79,28 @@ void parseCommand(char *buffer) case 'P': g.pid_stabilize_roll.kP((float)value / 1000); g.pid_stabilize_pitch.kP((float)value / 1000); - save_EEPROM_PID(); + g.pid_stabilize_pitch.save_gains(); break; case 'I': g.pid_stabilize_roll.kI((float)value / 1000); g.pid_stabilize_pitch.kI((float)value / 1000); - save_EEPROM_PID(); + g.pid_stabilize_pitch.save_gains(); break; case 'D': //g.pid_stabilize_roll.kD((float)value / 1000); //g.pid_stabilize_pitch.kD((float)value / 1000); - //save_EEPROM_PID(); break; case 'X': g.pid_stabilize_roll.imax(value * 100); g.pid_stabilize_pitch.imax(value * 100); - save_EEPROM_PID(); + g.pid_stabilize_pitch.save_gains(); break; case 'R': - stabilize_dampener = (float)value / 1000; - save_EEPROM_PID(); + g.stabilize_dampener.set_and_save((float)value / 1000); break; } init_pids(); diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde index ef7ef5f4bd..1043bbd2eb 100644 --- a/ArduCopterMega/sensors.pde +++ b/ArduCopterMega/sensors.pde @@ -6,7 +6,7 @@ void init_pressure_ground(void) for(int i = 0; i < 300; i++){ // We take some readings... delay(20); APM_BMP085.Read(); // Get initial data from absolute pressure sensor - abs_pressure_ground = (abs_pressure_ground * 9l + APM_BMP085.Press) / 10l; + ground_pressure = (ground_pressure * 9l + APM_BMP085.Press) / 10l; ground_temperature = (ground_temperature * 9 + APM_BMP085.Temp) / 10; } abs_pressure = APM_BMP085.Press; @@ -19,7 +19,7 @@ void read_barometer(void){ //abs_pressure = (abs_pressure + APM_BMP085.Press) >> 1; // Small filtering abs_pressure = ((float)abs_pressure * .7) + ((float)APM_BMP085.Press * .3); // large filtering - scaling = (float)abs_pressure_ground / (float)abs_pressure; + scaling = (float)ground_pressure / (float)abs_pressure; temp = ((float)ground_temperature / 10.f) + 273.15; x = log(scaling) * temp * 29271.267f; current_loc.alt = (long)(x / 10) + home.alt;// + baro_offset; // Pressure altitude in centimeters diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 21e83e1c66..9abf941f5e 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -15,7 +15,7 @@ static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv); static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv); -// Command/function table for the setup menu +// Command/function table for the setup menu const struct Menu::command setup_menu_commands[] PROGMEM = { // command function called // ======= =============== @@ -61,7 +61,7 @@ setup_show(uint8_t argc, const Menu::arg *argv) uint8_t i; // clear the area print_blanks(8); - + report_radio(); report_frame(); report_current(); @@ -88,14 +88,14 @@ setup_factory(uint8_t argc, const Menu::arg *argv) do { c = Serial.read(); } while (-1 == c); - + if (('y' != c) && ('Y' != c)) return(-1); - + //zero_eeprom(); default_gains(); - - + + // setup default values default_waypoint_info(); default_nav(); @@ -106,7 +106,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv) default_logs(); default_current(); print_done(); - + // finish // ------ return(0); @@ -119,12 +119,12 @@ setup_radio(uint8_t argc, const Menu::arg *argv) { Serial.println("\n\nRadio Setup:"); uint8_t i; - + for(i = 0; i < 100;i++){ delay(20); read_radio(); } - + if(g.rc_1.radio_in < 500){ while(1){ Serial.printf_P(PSTR("\nNo radio; Check connectors.")); @@ -132,7 +132,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) // stop here } } - + g.rc_1.radio_min = g.rc_1.radio_in; g.rc_2.radio_min = g.rc_2.radio_in; g.rc_3.radio_min = g.rc_3.radio_in; @@ -163,7 +163,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: ")); while(1){ - + delay(20); // Filters radio input - adjust filters in the radio.pde file // ---------------------------------------------------------- @@ -177,11 +177,11 @@ setup_radio(uint8_t argc, const Menu::arg *argv) g.rc_6.update_min_max(); g.rc_7.update_min_max(); g.rc_8.update_min_max(); - + if(Serial.available() > 0){ //g.rc_3.radio_max += 250; Serial.flush(); - + save_EEPROM_radio(); //delay(100); // double checking @@ -209,9 +209,9 @@ setup_motors(uint8_t argc, const Menu::arg *argv) print_hit_enter(); delay(1000); - + int out_min = g.rc_3.radio_min + 70; - + while(1){ @@ -222,28 +222,28 @@ setup_motors(uint8_t argc, const Menu::arg *argv) motor_out[CH_3] = g.rc_3.radio_min; motor_out[CH_4] = g.rc_3.radio_min; - - - if(frame_type == PLUS_FRAME){ + + + if(g.frame_type == PLUS_FRAME){ if(g.rc_1.control_in > 0){ motor_out[CH_1] = out_min; Serial.println("0"); - + }else if(g.rc_1.control_in < 0){ motor_out[CH_2] = out_min; Serial.println("1"); } - + if(g.rc_2.control_in > 0){ motor_out[CH_4] = out_min; Serial.println("3"); - + }else if(g.rc_2.control_in < 0){ motor_out[CH_3] = out_min; Serial.println("2"); } - }else if(frame_type == X_FRAME){ + }else if(g.frame_type == X_FRAME){ // lower right if((g.rc_1.control_in > 0) && (g.rc_2.control_in > 0)){ @@ -264,36 +264,36 @@ setup_motors(uint8_t argc, const Menu::arg *argv) motor_out[CH_1] = out_min; Serial.println("0"); } - - }else if(frame_type == TRI_FRAME){ + + }else if(g.frame_type == TRI_FRAME){ if(g.rc_1.control_in > 0){ motor_out[CH_1] = out_min; - + }else if(g.rc_1.control_in < 0){ motor_out[CH_2] = out_min; } - + if(g.rc_2.control_in > 0){ - motor_out[CH_4] = out_min; + motor_out[CH_4] = out_min; } if(g.rc_4.control_in > 0){ g.rc_4.servo_out = 2000; - + }else if(g.rc_4.control_in < 0){ g.rc_4.servo_out = -2000; } - + g.rc_4.calc_pwm(); motor_out[CH_3] = g.rc_4.radio_out; } - + if(g.rc_3.control_in > 0){ APM_RC.OutputCh(CH_1, g.rc_3.radio_in); APM_RC.OutputCh(CH_2, g.rc_3.radio_in); APM_RC.OutputCh(CH_3, g.rc_3.radio_in); - if(frame_type != TRI_FRAME) + if(g.frame_type != TRI_FRAME) APM_RC.OutputCh(CH_4, g.rc_3.radio_in); }else{ APM_RC.OutputCh(CH_1, motor_out[CH_1]); @@ -301,7 +301,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv) APM_RC.OutputCh(CH_3, motor_out[CH_3]); APM_RC.OutputCh(CH_4, motor_out[CH_4]); } - + if(Serial.available() > 0){ return (0); } @@ -312,9 +312,9 @@ static int8_t setup_accel(uint8_t argc, const Menu::arg *argv) { Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n")); - + imu.init_accel(); - imu.print_accel_offsets(); + print_accel_offsets(); report_imu(); return(0); @@ -325,14 +325,14 @@ setup_pid(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("default"))) { default_gains(); - + }else if (!strcmp_P(argv[1].str, PSTR("s_kp"))) { g.pid_stabilize_roll.kP(argv[2].f); g.pid_stabilize_pitch.kP(argv[2].f); save_EEPROM_PID(); - + }else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) { - stabilize_dampener = argv[2].f; + g.stabilize_dampener = argv[2].f; save_EEPROM_PID(); }else if (!strcmp_P(argv[1].str, PSTR("y_kp"))) { @@ -353,7 +353,7 @@ setup_pid(uint8_t argc, const Menu::arg *argv) }else{ default_gains(); } - + report_gains(); } @@ -362,20 +362,20 @@ static int8_t setup_flightmodes(uint8_t argc, const Menu::arg *argv) { byte switchPosition, oldSwitchPosition, mode; - + Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes.")); print_hit_enter(); trim_radio(); - + while(1){ delay(20); - read_radio(); + read_radio(); switchPosition = readSwitch(); - + // look for control switch change if (oldSwitchPosition != switchPosition){ - + mode = g.flight_modes[switchPosition]; mode = constrain(mode, 0, NUM_MODES-1); @@ -383,12 +383,12 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) print_switch(switchPosition, mode); // Remember switch position - oldSwitchPosition = switchPosition; + oldSwitchPosition = switchPosition; } // look for stick input if (radio_input_switch() == true){ - mode++; + mode++; if(mode >= NUM_MODES) mode = 0; @@ -398,7 +398,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) // print new mode print_switch(switchPosition, mode); } - + // escape hatch if(Serial.available() > 0){ save_EEPROM_flight_modes(); @@ -412,8 +412,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) static int8_t setup_declination(uint8_t argc, const Menu::arg *argv) { - mag_declination = argv[1].f; - save_EEPROM_mag_declination(); + compass.set_declination(radians(argv[1].f)); report_compass(); } @@ -430,16 +429,16 @@ setup_compass(uint8_t argc, const Menu::arg *argv) if (!strcmp_P(argv[1].str, PSTR("on"))) { g.compass_enabled = true; init_compass(); - + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { g.compass_enabled = false; - + } else { Serial.printf_P(PSTR("\nOptions:[on,off]\n")); report_compass(); return 0; } - + save_EEPROM_mag(); report_compass(); return 0; @@ -449,23 +448,23 @@ static int8_t setup_frame(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("+"))) { - frame_type = PLUS_FRAME; + g.frame_type = PLUS_FRAME; } else if (!strcmp_P(argv[1].str, PSTR("x"))) { - frame_type = X_FRAME; - + g.frame_type = X_FRAME; + } else if (!strcmp_P(argv[1].str, PSTR("tri"))) { - frame_type = TRI_FRAME; + g.frame_type = TRI_FRAME; } else if (!strcmp_P(argv[1].str, PSTR("hexa"))) { - frame_type = HEXA_FRAME; + g.frame_type = HEXA_FRAME; } else { Serial.printf_P(PSTR("\nOptions:[+, x, tri, hexa]\n")); report_frame(); return 0; } - + save_EEPROM_frame(); report_frame(); return 0; @@ -475,22 +474,22 @@ static int8_t setup_current(uint8_t argc, const Menu::arg *argv) { if (!strcmp_P(argv[1].str, PSTR("on"))) { - current_enabled = true; + g.current_enabled.set(true); save_EEPROM_mag(); - + } else if (!strcmp_P(argv[1].str, PSTR("off"))) { - current_enabled = false; + g.current_enabled.set(false); save_EEPROM_mag(); - + } else if(argv[1].i > 10){ milliamp_hours = argv[1].i; - + } else { Serial.printf_P(PSTR("\nOptions:[on, off, mAh]\n")); report_current(); return 0; } - + save_EEPROM_current(); report_current(); return 0; @@ -509,7 +508,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference compass.set_declination(ToRad(DECLINATION)); // set local difference between magnetic north and true north - //int counter = 0; + //int counter = 0; float _min[3], _max[3], _offset[3]; while(1){ @@ -537,15 +536,15 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) offset[0] = -(_max[0] + _min[0]) / 2; offset[1] = -(_max[1] + _min[1]) / 2; offset[2] = -(_max[2] + _min[2]) / 2; - - // display all to user + + // display all to user Serial.printf_P(PSTR("Heading: ")); Serial.print(ToDeg(compass.heading)); Serial.print(" \t("); Serial.print(compass.mag_x); Serial.print(","); Serial.print(compass.mag_y); - Serial.print(","); + Serial.print(","); Serial.print(compass.mag_z); Serial.print(")\t offsets("); Serial.print(offset[0]); @@ -554,18 +553,18 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) Serial.print(","); Serial.print(offset[2]); Serial.println(")"); - + if(Serial.available() > 0){ - + //mag_offset_x = offset[0]; //mag_offset_y = offset[1]; //mag_offset_z = offset[2]; - + //save_EEPROM_mag_offset(); - + // set offsets to account for surrounding interference //compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); - + report_compass(); break; } @@ -579,7 +578,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) /***************************************************************************/ void default_waypoint_info() -{ +{ g.waypoint_radius = 4; //TODO: Replace this quick fix with a real way to define wp_radius g.loiter_radius = 30; //TODO: Replace this quick fix with a real way to define loiter_radius save_EEPROM_waypoint_info(); @@ -599,14 +598,14 @@ default_nav() void default_alt_hold() { - alt_to_hold = -1; + g.RTL_altitude.set(-1); save_EEPROM_alt_RTL(); } void default_frame() { - frame_type = PLUS_FRAME; + g.frame_type = PLUS_FRAME; save_EEPROM_frame(); } @@ -614,7 +613,7 @@ void default_current() { milliamp_hours = 2000; - current_enabled = false; + g.current_enabled.set(false); save_EEPROM_current(); } @@ -633,12 +632,12 @@ default_flight_modes() void default_throttle() { - g.throttle_min = THROTTLE_MIN; - g.throttle_max = THROTTLE_MAX; - g.throttle_cruise = THROTTLE_CRUISE; - throttle_failsafe_enabled = THROTTLE_FAILSAFE; - throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION; - throttle_failsafe_value = THROTTLE_FS_VALUE; + g.throttle_min = THROTTLE_MIN; + g.throttle_max = THROTTLE_MAX; + g.throttle_cruise = THROTTLE_CRUISE; + g.throttle_failsafe_enabled = THROTTLE_FAILSAFE; + g.throttle_failsafe_action = THROTTLE_FAILSAFE_ACTION; + g.throttle_failsafe_value = THROTTLE_FS_VALUE; save_EEPROM_throttle(); } @@ -647,7 +646,7 @@ void default_logs() // convenience macro for testing LOG_* and setting LOGBIT_* #define LOGBIT(_s) (LOG_ ## _s ? LOGBIT_ ## _s : 0) - g.log_bitmask = + g.log_bitmask = LOGBIT(ATTITUDE_FAST) | LOGBIT(ATTITUDE_MED) | LOGBIT(GPS) | @@ -659,7 +658,7 @@ void default_logs() LOGBIT(CMD) | LOGBIT(CURRENT); #undef LOGBIT - + save_EEPROM_logs(); } @@ -672,7 +671,7 @@ default_gains() g.pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I); g.pid_acro_rate_roll.kD(0); g.pid_acro_rate_roll.imax(ACRO_RATE_ROLL_IMAX * 100); - + g.pid_acro_rate_pitch.kP(ACRO_RATE_PITCH_P); g.pid_acro_rate_pitch.kI(ACRO_RATE_PITCH_I); g.pid_acro_rate_pitch.kD(0); @@ -689,7 +688,7 @@ default_gains() g.pid_stabilize_roll.kI(STABILIZE_ROLL_I); g.pid_stabilize_roll.kD(0); g.pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100); - + g.pid_stabilize_pitch.kP(STABILIZE_PITCH_P); g.pid_stabilize_pitch.kI(STABILIZE_PITCH_I); g.pid_stabilize_pitch.kD(0); @@ -704,10 +703,10 @@ default_gains() // custom dampeners // roll pitch - stabilize_dampener = STABILIZE_DAMPENER; - + g.stabilize_dampener = STABILIZE_DAMPENER; + //yaw - hold_yaw_dampener = HOLD_YAW_DAMPENER; + g.hold_yaw_dampener = HOLD_YAW_DAMPENER; // navigation @@ -730,7 +729,7 @@ default_gains() g.pid_sonar_throttle.kI(THROTTLE_SONAR_I); g.pid_sonar_throttle.kD(THROTTLE_SONAR_D); g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX); - + save_EEPROM_PID(); } @@ -746,7 +745,7 @@ void report_current() read_EEPROM_current(); Serial.printf_P(PSTR("Current Sensor\n")); print_divider(); - print_enabled(current_enabled); + print_enabled(g.current_enabled.get()); Serial.printf_P(PSTR("mah: %d"),milliamp_hours); print_blanks(1); @@ -760,17 +759,17 @@ void report_frame() Serial.printf_P(PSTR("Frame\n")); print_divider(); - - if(frame_type == X_FRAME) + + if(g.frame_type == X_FRAME) Serial.printf_P(PSTR("X ")); - else if(frame_type == PLUS_FRAME) + else if(g.frame_type == PLUS_FRAME) Serial.printf_P(PSTR("Plus ")); - else if(frame_type == TRI_FRAME) + else if(g.frame_type == TRI_FRAME) Serial.printf_P(PSTR("TRI ")); - else if(frame_type == HEXA_FRAME) + else if(g.frame_type == HEXA_FRAME) Serial.printf_P(PSTR("HEXA ")); - Serial.printf_P(PSTR("frame (%d)"), (int)frame_type); + Serial.printf_P(PSTR("frame (%d)"), (int)g.frame_type); print_blanks(1); } @@ -807,10 +806,10 @@ void report_gains() print_PID(&g.pid_stabilize_pitch); Serial.printf_P(PSTR("yaw:\n")); print_PID(&g.pid_yaw); - - Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), stabilize_dampener); - Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), hold_yaw_dampener); - + + Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), g.stabilize_dampener); + Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), g.hold_yaw_dampener); + // Nav Serial.printf_P(PSTR("Nav:\nlat:\n")); print_PID(&g.pid_nav_lat); @@ -833,9 +832,9 @@ void report_xtrack() Serial.printf_P(PSTR("XTRACK: %4.2f\n" "XTRACK angle: %d\n" "PITCH_MAX: %ld"), - (float)g.crosstrack_gain, - (int)g.crosstrack_entry_angle, - (long)g.pitch_max); + g.crosstrack_gain, + g.crosstrack_entry_angle, + g.pitch_max); print_blanks(1); } @@ -851,11 +850,11 @@ void report_throttle() "cruise: %d\n" "failsafe_enabled: %d\n" "failsafe_value: %d"), - (int)g.throttle_min, - (int)g.throttle_max, - (int)g.throttle_cruise, - throttle_failsafe_enabled, - throttle_failsafe_value); + g.throttle_min, + g.throttle_max, + g.throttle_cruise, + g.throttle_failsafe_enabled, + g.throttle_failsafe_value); print_blanks(1); } @@ -865,43 +864,45 @@ void report_imu() Serial.printf_P(PSTR("IMU\n")); print_divider(); - imu.print_gyro_offsets(); - imu.print_accel_offsets(); + print_gyro_offsets(); + print_accel_offsets(); print_blanks(1); } void report_compass() { - print_blanks(2); + print_blanks(2); Serial.printf_P(PSTR("Compass\n")); print_divider(); read_EEPROM_compass(); - read_EEPROM_compass_declination(); - read_EEPROM_compass_offset(); + //read_EEPROM_compass_declination(); + //read_EEPROM_compass_offset(); print_enabled(g.compass_enabled); - + // mag declination - Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), - mag_declination); + Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), + degrees(compass.get_declination())); + + Vector3f offsets = compass.get_offsets(); // mag offsets Serial.printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f"), - mag_offset_x, - mag_offset_y, - mag_offset_z); + offsets.x, + offsets.y, + offsets.z); print_blanks(1); } void report_flight_modes() { - print_blanks(2); + print_blanks(2); Serial.printf_P(PSTR("Flight modes\n")); print_divider(); read_EEPROM_flight_modes(); - + for(int i = 0; i < 6; i++ ){ print_switch(i, g.flight_modes[i]); } @@ -912,23 +913,23 @@ void report_flight_modes() // CLI utilities /***************************************************************************/ -void +void print_PID(PID * pid) { Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), pid->kP(), pid->kI(), pid->kD(), (long)pid->imax()); } -void +void print_radio_values() { - Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max); - Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max); - Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max); - Serial.printf_P(PSTR("CH4: %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_max); - Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max); - Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max); - Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max); - Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max); + Serial.printf_P(PSTR("CH1: %d | %d\n"), g.rc_1.radio_min, g.rc_1.radio_max); + Serial.printf_P(PSTR("CH2: %d | %d\n"), g.rc_2.radio_min, g.rc_2.radio_max); + Serial.printf_P(PSTR("CH3: %d | %d\n"), g.rc_3.radio_min, g.rc_3.radio_max); + Serial.printf_P(PSTR("CH4: %d | %d\n"), g.rc_4.radio_min, g.rc_4.radio_max); + Serial.printf_P(PSTR("CH5: %d | %d\n"), g.rc_5.radio_min, g.rc_5.radio_max); + Serial.printf_P(PSTR("CH6: %d | %d\n"), g.rc_6.radio_min, g.rc_6.radio_max); + Serial.printf_P(PSTR("CH7: %d | %d\n"), g.rc_7.radio_min, g.rc_7.radio_max); + Serial.printf_P(PSTR("CH8: %d | %d\n"), g.rc_8.radio_min, g.rc_8.radio_max); } void @@ -968,7 +969,7 @@ boolean radio_input_switch(void) { static byte bouncer; - + if (abs(g.rc_1.radio_in - g.rc_1.radio_trim) > 200) bouncer = 10; @@ -1005,3 +1006,26 @@ void print_enabled(boolean b) +void +print_accel_offsets(void) +{ + //Serial.print("Accel offsets: "); + //Serial.print(imu._adc_offset[3], 2); + //Serial.print(", "); + //Serial.print(_adc_offset[4], 2); + //Serial.print(", "); + //Serial.println(_adc_offset[5], 2); +} + +void +print_gyro_offsets(void) +{ + //Serial.print("Gyro offsets: "); + //Serial.print(_adc_offset[0], 2); + //Serial.print(", "); + //Serial.print(_adc_offset[1], 2); + //Serial.print(", "); + //Serial.println(_adc_offset[2], 2); +} + + diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index de0e4053c9..426c97b713 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -85,7 +85,7 @@ void init_ardupilot() #endif Serial.printf_P(PSTR("\n\n" - "Init ArduCopterMega 1.0.2 Public Alpha\n\n" + "Init ArduCopterMega 1.0.3 Public Alpha\n\n" #if TELEMETRY_PORT == 3 "Telemetry is on the xbee port\n" #endif @@ -125,6 +125,9 @@ void init_ardupilot() g_gps = &GPS; g_gps->init(); + + imu.init(IMU::WARM_START); // offsets are loaded from EEPROM + if(g.compass_enabled) init_compass(); @@ -243,7 +246,7 @@ void startup_ground(void) // Warm up and read Gyro offsets // ----------------------------- - imu.init_gyro(); + imu.init(IMU::COLD_START); // Save the settings for in-air restart // ------------------------------------ @@ -403,10 +406,9 @@ void init_compass() { dcm.set_compass(&compass); - compass.init(); + bool junkbool = compass.init(); compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft - compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z); // set offsets to account for surrounding interference - compass.set_declination(ToRad(mag_declination)); // set local difference between magnetic north and true north + Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference } diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 338dc1c6f8..fd902832d7 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -686,15 +686,15 @@ test_relay(uint8_t argc, const Menu::arg *argv) delay(1000); while(1){ - Serial.println("Relay A"); - relay_A(); + Serial.println("Relay on"); + relay_on(); delay(3000); if(Serial.available() > 0){ return (0); } - Serial.println("Relay B"); - relay_B(); + Serial.println("Relay off"); + relay_off(); delay(3000); if(Serial.available() > 0){ return (0); @@ -764,7 +764,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv) sum += abs_pressure; delay(10); } - abs_pressure_ground = (float)sum / 100.0; + ground_pressure = (float)sum / 100.0; */ home.alt = 0; @@ -795,7 +795,7 @@ test_pressure(uint8_t argc, const Menu::arg *argv) current_loc.alt, next_WP.alt, altitude_error, - g., + g.throttle_cruise, g.rc_3.servo_out); /*