diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 46336acea7..a2c7a70d69 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -272,7 +272,7 @@ float cos_roll_x = 1; float cos_pitch_x = 1; float cos_yaw_x = 1; float sin_pitch_y, sin_yaw_y, sin_roll_y; -float sin_nav_y, cos_nav_x; // used in calc_waypoint_nav +float sin_nav_y, cos_nav_x; // used in calc_nav_output XXX move to local funciton bool simple_bearing_is_set = false; long initial_simple_bearing; // used for Simple mode @@ -922,20 +922,11 @@ void update_current_flight_mode(void) } switch(command_must_ID){ - //case MAV_CMD_NAV_TAKEOFF: - // break; - - //case MAV_CMD_NAV_LAND: - // break; - default: // Output Pitch, Roll, Yaw and Throttle // ------------------------------------ auto_yaw(); - //if(invalid_nav) - //calc_waypoint_nav(); - // mix in user control control_nav_mixer(); @@ -1107,6 +1098,13 @@ void update_current_flight_mode(void) adjust_altitude(); + #if AUTO_RESET_LOITER == 1 + if((g.rc_2.control_in + g.rc_1.control_in) != 0){ + // reset LOITER to current position + next_WP = current_loc; + } + #endif + // Output Pitch, Roll, Yaw and Throttle // ------------------------------------ @@ -1136,66 +1134,51 @@ void update_current_flight_mode(void) } } -void update_nav_wp() -{ - if(wp_control == LOITER_MODE){ - // calc a pitch to the target - calc_loiter_nav(); - - // rotate pitch and roll to the copter frame of reference - calc_loiter_output(); - - } else { - // how far are we from the ideal trajectory? - // this pushes us back on course - update_crosstrack(); - - // calc a rate dampened pitch to the target - calc_rate_nav(); - - // rotate that pitch to the copter frame of reference - calc_nav_output(); - } - -} - // called after a GPS read void update_navigation() { // wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS // ------------------------------------------------------------------------ + switch(control_mode){ + case AUTO: + verify_commands(); - // distance and bearing calcs only - if(control_mode == AUTO){ - verify_commands(); + // note: wp_control is handled by commands_logic - update_nav_wp(); + // calculates desired Yaw + update_nav_yaw(); - // this tracks a location so the copter is always pointing towards it. - if(yaw_tracking == MAV_ROI_LOCATION){ - nav_yaw = get_bearing(¤t_loc, &target_WP); + // calculates the desired Roll and Pitch + update_nav_wp(); + break; - }else if(yaw_tracking == MAV_ROI_WPNEXT){ - nav_yaw = target_bearing; - } + case LOITER: + case RTL: + // are we Traversing or Loitering? + wp_control = (wp_distance < 10) ? LOITER_MODE : WP_MODE; - }else{ + // calculates desired Yaw + update_nav_yaw(); - switch(control_mode){ - case LOITER: - wp_control = LOITER_MODE; - update_nav_wp(); - break; + // calculates the desired Roll and Pitch + update_nav_wp(); + break; + + #if YAW_HACK == 1 + case SIMPLE: + // calculates desired Yaw + // exprimental_hack + if(g.rc_6.control_in > 900) + update_nav_yaw(); + if(g.rc_6.control_in < 100){ + nav_yaw = dcm.yaw_sensor; + } + break; + #endif - case RTL: - wp_control = (wp_distance < 700) ? LOITER_MODE : WP_MODE; - update_nav_wp(); - break; - } } } - void read_AHRS(void) { // Perform IMU calculations and get attitude info @@ -1337,3 +1320,35 @@ void tuning(){ #endif } +void update_nav_wp() +{ + if(wp_control == LOITER_MODE){ + // calc a pitch to the target + calc_loiter_nav(); + + // rotate pitch and roll to the copter frame of reference + calc_loiter_output(); + + } else { + // how far are we from the ideal trajectory? + // this pushes us back on course + update_crosstrack(); + + // calc a rate dampened pitch to the target + calc_rate_nav(); + + // rotate that pitch to the copter frame of reference + calc_nav_output(); + } +} + +void update_nav_yaw() +{ + // this tracks a location so the copter is always pointing towards it. + if(yaw_tracking == MAV_ROI_LOCATION){ + nav_yaw = get_bearing(¤t_loc, &target_WP); + + }else if(yaw_tracking == MAV_ROI_WPNEXT){ + nav_yaw = target_bearing; + } +} diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index a0440aae1f..9996d60f61 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -132,12 +132,12 @@ void calc_nav_throttle() if(error < 0){ // try and prevent rapid fall - scaler = (altitude_sensor == BARO) ? .5 : .9; + //scaler = (altitude_sensor == BARO) ? 1 : 1; } if(altitude_sensor == BARO){ nav_throttle = g.pid_baro_throttle.get_pid(error, dTnav2, scaler); // .25 - nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 100); + nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 140); }else{ nav_throttle = g.pid_sonar_throttle.get_pid(error, dTnav2, scaler); // .5 nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 150); @@ -260,7 +260,7 @@ output_yaw_with_hold(boolean hold) }else{ // RATE control // Hein, 5/21/11 - long error = ((long)g.rc_4.control_in * 6) - (rate * 3); // control is += 6000 * 6 = 36000 + long error = ((long)g.rc_4.control_in * 6) - (rate * 2); // control is += 6000 * 6 = 36000 g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520 yaw_debug = YAW_RATE; // 2 } diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 00e6d0e982..dd20095c17 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -52,26 +52,21 @@ print_log_menu(void) byte last_log_num = get_num_logs(); Serial.printf_P(PSTR("logs enabled: ")); + if (0 == g.log_bitmask) { Serial.printf_P(PSTR("none")); }else{ - // Macro to make the following code a bit easier on the eye. - // Pass it the capitalised name of the log option, as defined - // in defines.h but without the LOG_ prefix. It will check for - // the bit being set and print the name of the log option to suit. - #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s)) - PLOG(ATTITUDE_FAST); - PLOG(ATTITUDE_MED); - PLOG(GPS); - PLOG(PM); - PLOG(CTUN); - PLOG(NTUN); - PLOG(MODE); - PLOG(RAW); - PLOG(CMD); - PLOG(CURRENT); - #undef PLOG + if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Serial.printf_P(PSTR(" ATTITUDE_FAST")); + if (g.log_bitmask & MASK_LOG_ATTITUDE_MED) Serial.printf_P(PSTR(" ATTITUDE_MED")); + if (g.log_bitmask & MASK_LOG_GPS) Serial.printf_P(PSTR(" GPS")); + if (g.log_bitmask & MASK_LOG_PM) Serial.printf_P(PSTR(" PM")); + if (g.log_bitmask & MASK_LOG_CTUN) Serial.printf_P(PSTR(" CTUN")); + if (g.log_bitmask & MASK_LOG_NTUN) Serial.printf_P(PSTR(" NTUN")); + if (g.log_bitmask & MASK_LOG_RAW) Serial.printf_P(PSTR(" RAW")); + if (g.log_bitmask & MASK_LOG_CMD) Serial.printf_P(PSTR(" CMD")); + if (g.log_bitmask & MASK_LOG_CURRENT) Serial.printf_P(PSTR(" CURRENT")); } + Serial.println(); if (last_log_num == 0) { @@ -108,13 +103,13 @@ dump_log(uint8_t argc, const Menu::arg *argv) } get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end); - Serial.printf_P(PSTR("Dumping Log number %d, start %d, end %d\n"), + /*Serial.printf_P(PSTR("Dumping Log number %d, start %d, end %d\n"), dump_log, dump_log_start, dump_log_end); - + */ Log_Read(dump_log_start, dump_log_end); - Serial.printf_P(PSTR("Done\n")); + //Serial.printf_P(PSTR("Done\n")); } static int8_t @@ -354,74 +349,6 @@ int find_last_log_page(int bottom_page) return top_page; } -void Log_Write_Nav_Tuning() -{ - Matrix3f tempmat = dcm.get_dcm_matrix(); - - DataFlash.WriteByte(HEAD_BYTE1); - DataFlash.WriteByte(HEAD_BYTE2); - DataFlash.WriteByte(LOG_NAV_TUNING_MSG); - - DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); // 1 - DataFlash.WriteInt((int)wp_distance); // 2 - DataFlash.WriteByte(wp_verify_byte); // 3 - DataFlash.WriteInt((int)(target_bearing/100)); // 4 - DataFlash.WriteInt((int)(nav_bearing/100)); // 5 - - DataFlash.WriteInt((int)(g.rc_3.servo_out)); // 6 - DataFlash.WriteByte(altitude_sensor); // 7 - DataFlash.WriteInt((int)sonar_alt); // 8 - DataFlash.WriteInt((int)baro_alt); // 9 - - DataFlash.WriteInt((int)home.alt); // 10 - DataFlash.WriteInt((int)next_WP.alt); // 11 - DataFlash.WriteInt((int)altitude_error); // 12 - - DataFlash.WriteInt((int)(wrap_360(ToDeg(compass.heading)*100)/100)); // Just a temp hack - DataFlash.WriteLong(compass.last_update); // Just a temp hack - DataFlash.WriteInt((int)(tempmat.b.x*1000)); // Just a temp hack - DataFlash.WriteInt((int)(compass.heading_x*1000)); // Just a temp hack - DataFlash.WriteInt((int)(tempmat.a.x*1000)); // Just a temp hack - DataFlash.WriteInt((int)(compass.heading_y*1000)); // Just a temp hack - - DataFlash.WriteByte(END_BYTE); -} - // 1 2 3 4 5 6 7 8 9 10 11 -//NTUN, 236, 0, 132, 10, 0, 0, 29, 2963, 16545, 16682, 108 - -void Log_Read_Nav_Tuning() -{ - // 1 2 3 4 5 6 7 8 9 10 11 - Serial.printf_P(PSTR( "NTUN, %d, %d, %d, " - "%d, %d, " - "%d, %d, %d, %d, " - "%d, %d, %d, " - "%d, %ld, %4.4f, %4.4f, %4.4f, %4.4f\n"), - DataFlash.ReadInt(), //yaw sensor - DataFlash.ReadInt(), //distance - DataFlash.ReadByte(), //bitmask - - DataFlash.ReadInt(), //target bearing - DataFlash.ReadInt(), //nav bearing - - DataFlash.ReadInt(), //throttle - DataFlash.ReadByte(), //Alt sensor - DataFlash.ReadInt(), //Sonar - DataFlash.ReadInt(), //Baro - - DataFlash.ReadInt(), //home.alt - DataFlash.ReadInt(), //Next_alt - DataFlash.ReadInt(), //Alt Error - - DataFlash.ReadInt(), - DataFlash.ReadLong(), - (float)DataFlash.ReadInt()/1000, - (float)DataFlash.ReadInt()/1000, - (float)DataFlash.ReadInt()/1000, - (float)DataFlash.ReadInt()/1000); -} - - // Write an GPS packet. Total length : 30 bytes void Log_Write_GPS() { @@ -516,6 +443,31 @@ void Log_Read_Current() DataFlash.ReadInt()); } +void Log_Write_Nav_Tuning() +{ + Matrix3f tempmat = dcm.get_dcm_matrix(); + + DataFlash.WriteByte(HEAD_BYTE1); + DataFlash.WriteByte(HEAD_BYTE2); + DataFlash.WriteByte(LOG_NAV_TUNING_MSG); + + DataFlash.WriteInt((int)wp_distance); // 1 + DataFlash.WriteByte(wp_verify_byte); // 2 + DataFlash.WriteInt((int)(target_bearing/100)); // 3 + DataFlash.WriteInt((int)(nav_bearing/100)); // 4 + + DataFlash.WriteByte(END_BYTE); +} + +void Log_Read_Nav_Tuning() +{ + // 1 2 3 4 + Serial.printf_P(PSTR( "NTUN, %d, %d, %d, %d\n"), + DataFlash.ReadInt(), //distance + DataFlash.ReadByte(), //bitmask + DataFlash.ReadInt(), //target bearing + DataFlash.ReadInt()); //nav bearing +} // Write a control tuning packet. Total length : 22 bytes #if HIL_MODE != HIL_MODE_ATTITUDE @@ -526,29 +478,24 @@ void Log_Write_Control_Tuning() DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); // Control - DataFlash.WriteInt((int)(g.rc_3.control_in)); - DataFlash.WriteInt((int)(g.rc_3.servo_out)); DataFlash.WriteInt((int)(g.rc_4.control_in)); DataFlash.WriteInt((int)(g.rc_4.servo_out)); // yaw DataFlash.WriteByte(yaw_debug); - DataFlash.WriteInt((int)yaw_error); DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); + DataFlash.WriteInt((int)(nav_yaw/100)); + DataFlash.WriteInt((int)yaw_error); DataFlash.WriteInt((int)(omega.z * 1000)); - // Pitch/roll - DataFlash.WriteInt((int)(dcm.pitch_sensor/100)); - DataFlash.WriteInt((int)(dcm.roll_sensor/100)); + // Alt hold + DataFlash.WriteInt((int)(g.rc_3.servo_out)); + DataFlash.WriteInt((int)sonar_alt); // + DataFlash.WriteInt((int)baro_alt); // - DataFlash.WriteInt((int)g.throttle_cruise); + DataFlash.WriteInt((int)next_WP.alt); // + DataFlash.WriteInt((int)altitude_error); // DataFlash.WriteInt((int)g.pid_baro_throttle.get_integrator()); - DataFlash.WriteInt((int)g.pid_sonar_throttle.get_integrator()); - - // Position - //DataFlash.WriteInt((int)dcm.pitch_sensor); - //DataFlash.WriteInt((int)dcm.roll_sensor); - //DataFlash.WriteInt((int)(dcm.yaw_sensor/10)); DataFlash.WriteByte(END_BYTE); } @@ -557,28 +504,26 @@ void Log_Write_Control_Tuning() // Read an control tuning packet void Log_Read_Control_Tuning() { - Serial.printf_P(PSTR( "CTUN, %d, %d, %d, %d, " - "%d, %d, %d, %1.4f, " - "%d, %d, " - "%d, %d, %d\n"), + Serial.printf_P(PSTR( "CTUN, %d, %d, " + "%d, %d, %d, %d, %1.4f, " + "%d, %d, %d, %d, %d, %d\n"), // Control DataFlash.ReadInt(), DataFlash.ReadInt(), - DataFlash.ReadInt(), - DataFlash.ReadInt(), // yaw (int)DataFlash.ReadByte(), DataFlash.ReadInt(), DataFlash.ReadInt(), + DataFlash.ReadInt(), (float)DataFlash.ReadInt() / 1000.0,// Gyro Rate - // Pitch/roll + // Alt Hold + DataFlash.ReadInt(), DataFlash.ReadInt(), DataFlash.ReadInt(), - // Alt Hold DataFlash.ReadInt(), DataFlash.ReadInt(), DataFlash.ReadInt()); @@ -700,6 +645,7 @@ void Log_Write_Mode(byte mode) DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_MODE_MSG); DataFlash.WriteByte(mode); + DataFlash.WriteInt((int)g.throttle_cruise); DataFlash.WriteByte(END_BYTE); } @@ -707,7 +653,8 @@ void Log_Write_Mode(byte mode) void Log_Read_Mode() { Serial.printf_P(PSTR("MOD:")); - Serial.println(flight_mode_strings[DataFlash.ReadByte()]); + Serial.print(flight_mode_strings[DataFlash.ReadByte()]); + Serial.printf_P(PSTR(", %d\n"),DataFlash.ReadInt()); } // Read a raw accel/gyro packet diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 9c09ce0921..8d9c71b4a1 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -227,6 +227,9 @@ void init_home() home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid home_is_set = true; + // to point yaw towards home until we set it with Mavlink + target_WP = home; + //Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt); // Save Home to EEPROM diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 5ce2d5fb0a..91a4f0844e 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "AC 2.0.8 Beta", main_menu_commands); +MENU(main_menu, "AC 2.0.9 Beta", main_menu_commands); void init_ardupilot() { diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 8c7a22d72c..a16c25ac4d 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -518,6 +518,7 @@ test_gps(uint8_t argc, const Menu::arg *argv) g_gps->longitude, g_gps->altitude/100, g_gps->num_sats); + g_gps->new_data = false; }else{ Serial.print("."); }