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

Warning!!! The recent build 1919 was tagged as sparkfun.

Browse Source 
The trunk is unstable, so please be careful. I've made a lot of changes based on the Sparkfun challange. Most include how we handle things like altitude, but the mission scripting has also been updated as well.


git-svn-id: https://arducopter.googlecode.com/svn/trunk@1920 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-04-25 05:12:59 +00:00
parent 16412abbde
commit 121cf0f658
19 changed files with 541 additions and 405 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopterMega/APM_Config.h
View File

@ -43,12 +43,12 @@
# define LOG_ATTITUDE_FAST DISABLED
# define LOG_ATTITUDE_MED DISABLED
# define LOG_GPS DISABLED
# define LOG_GPS ENABLED
# define LOG_PM DISABLED
# define LOG_CTUN ENABLED
# define LOG_NTUN DISABLED
# define LOG_MODE DISABLED
# define LOG_NTUN ENABLED
# define LOG_MODE ENABLED
# define LOG_RAW DISABLED
# define LOG_CMD DISABLED
# define LOG_CMD ENABLED
# define LOG_CURRENT DISABLED

123
ArduCopterMega/ArduCopterMega.pde
View File

@ -256,6 +256,8 @@ long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to t
int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate
float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1?
int last_ground_speed; // used to dampen navigation
byte command_must_index; // current command memory location
byte command_may_index; // current command memory location
byte command_must_ID; // current command ID
@ -310,7 +312,6 @@ int ground_temperature;
// ----------------------
int sonar_alt;
int baro_alt;
int baro_alt_offset;
byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
// flight mode specific
@ -325,10 +326,10 @@ byte yaw_tracking = TRACK_NONE; // no tracking, point at next wp, or at a tar
// Loiter management
// -----------------
long old_target_bearing; // deg * 100
int loiter_total; // deg : how many times to loiter * 360
int loiter_delta; // deg : how far we just turned
int loiter_sum; // deg : how far we have turned around a waypoint
long saved_target_bearing; // deg * 100
//int loiter_total; // deg : how many times to loiter * 360
//int loiter_delta; // deg : how far we just turned
//int loiter_sum; // deg : how far we have turned around a waypoint
long loiter_time; // millis : when we started LOITER mode
long loiter_time_max; // millis : how long to stay in LOITER mode
@ -342,6 +343,8 @@ long nav_lon; // for error calcs
int nav_throttle; // 0-1000 for throttle control
int nav_throttle_old; // for filtering
bool invalid_throttle; // used to control when we calculate nav_throttle
bool invalid_nav; // used to control when we calculate nav_throttle
bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
long command_yaw_start; // what angle were we to begin with
@ -591,12 +594,17 @@ void medium_loop()
// we call these regardless of GPS because of the rapid nature of the yaw sensor
// -----------------------------------------------------------------------------
if(wp_distance < 800){ // 8 meters
//if(wp_distance < 800){ // 8 meters
//if(g.rc_6.control_in > 500){ // 8 meters
calc_loiter_nav();
}else{
calc_waypoint_nav();
}
//calc_loiter_nav();
// calc_waypoint_nav();
//}else{
// calc_waypoint_nav();
//}
invalid_nav = true;
// replace with invalidater byte
//calc_waypoint_nav();
break;
@ -609,10 +617,22 @@ void medium_loop()
// --------------------------
update_alt();
// altitude smoothing
// ------------------
//calc_altitude_smoothing_error();
calc_altitude_error();
// invalidate the throttle hold value
// ----------------------------------
invalid_throttle = true;
// perform next command
// --------------------
if(control_mode == AUTO || control_mode == GCS_AUTO){
update_commands();
if(home_is_set){
update_commands();
}
}
break;
@ -790,7 +810,7 @@ void super_slow_loop()
temp.alt = gcs_simple.altitude;
temp.lat = gcs_simple.latitude;
temp.lng = gcs_simple.longitude;
set_wp_with_index(temp, gcs_simple.index);
set_command_with_index(temp, gcs_simple.index);
gcs_simple.ack();
*/
//}
@ -852,6 +872,12 @@ void update_current_flight_mode(void)
{
if(control_mode == AUTO){
// this is a hack to prevent run up of the throttle I term for alt hold
if(command_must_ID == MAV_CMD_NAV_TAKEOFF){
invalid_throttle = (g.rc_3.control_in != 0);
// make invalid_throttle false if we are waiting to take off.
}
switch(command_must_ID){
//case MAV_CMD_NAV_TAKEOFF:
// break;
@ -864,6 +890,9 @@ void update_current_flight_mode(void)
// ------------------------------------
auto_yaw();
//if(invalid_nav)
//calc_waypoint_nav();
// mix in user control
control_nav_mixer();
@ -871,6 +900,9 @@ void update_current_flight_mode(void)
output_stabilize_roll();
output_stabilize_pitch();
if(invalid_throttle)
calc_nav_throttle();
// apply throttle control
output_auto_throttle();
break;
@ -964,7 +996,9 @@ void update_current_flight_mode(void)
bearing_error);
*/
// get nav_pitch and nav_roll
calc_waypoint_nav();
calc_simple_nav();
calc_nav_output();
limit_nav_pitch_roll(4500);
}
// Output Pitch, Roll, Yaw and Throttle
@ -996,6 +1030,10 @@ void update_current_flight_mode(void)
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
if(invalid_throttle)
calc_nav_throttle();
// apply throttle control
output_auto_throttle();
@ -1012,6 +1050,9 @@ void update_current_flight_mode(void)
// ------------------------------------
auto_yaw();
if(invalid_throttle)
calc_nav_throttle();
// apply throttle control
output_auto_throttle();
@ -1034,6 +1075,9 @@ void update_current_flight_mode(void)
// -----------
output_manual_yaw();
if(invalid_throttle)
calc_nav_throttle();
// apply throttle control
output_auto_throttle();
@ -1063,6 +1107,16 @@ void update_navigation()
if(control_mode == AUTO || control_mode == GCS_AUTO){
verify_commands();
// calc a rate dampened pitch to the target
calc_rate_nav();
// rotate that pitch to the copter frame of reference
calc_nav_output();
//limit our copter pitch - this will change if we go to a fully rate limited approach.
limit_nav_pitch_roll(g.pitch_max.get());
// this tracks a location so the copter is always pointing towards it.
if(yaw_tracking & TRACK_TARGET_WP){
nav_yaw = get_bearing(&current_loc, &target_WP);
}
@ -1106,6 +1160,8 @@ void update_trig(void){
void update_alt()
{
altitude_sensor = BARO;
#if HIL_MODE == HIL_MODE_ATTITUDE
current_loc.alt = g_gps->altitude;
#else
@ -1114,40 +1170,36 @@ void update_alt()
baro_alt = read_barometer();
//filter out bad sonar reads
int temp = sonar.read();
//int temp = sonar.read();
if(abs(temp - sonar_alt) < 300){
sonar_alt = temp;
}
//if(abs(temp - sonar_alt) < 300){
// sonar_alt = temp;
//}
// correct alt for angle of the sonar
sonar_alt = (float)sonar_alt * (cos_pitch_x * cos_roll_x);
sonar_alt = sonar.read();
// output a light to show sonar is working
update_sonar_light(sonar_alt > 100);
// decide which sensor we're usings
if(sonar_alt < 500){ // less than 5m or 15 feet
altitude_sensor = SONAR;
// decide if we're using sonar
if (baro_alt < 1200){
// XXX this is a hack for now. it kills accuracy from GPS reading of altitude and focuses
// on altitude above flat ground.
baro_alt_offset = sonar_alt - baro_alt;
// correct alt for angle of the sonar
sonar_alt = (float)sonar_alt * (cos_pitch_x * cos_roll_x);
}else{
altitude_sensor = BARO;
if(sonar_alt < 500){
altitude_sensor = SONAR;
}
}
// calculate our altitude
if(altitude_sensor == BARO){
current_loc.alt = baro_alt + baro_alt_offset + home.alt;
current_loc.alt = baro_alt + home.alt;
}else{
current_loc.alt = sonar_alt + home.alt;
}
}else{
altitude_sensor = BARO;
baro_alt = read_barometer();
// no sonar altitude
current_loc.alt = baro_alt + home.alt;
@ -1155,17 +1207,6 @@ void update_alt()
//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
#endif
// altitude smoothing
// ------------------
//calc_altitude_smoothing_error();
calc_altitude_error();
// Amount of throttle to apply for hovering
// ----------------------------------------
calc_nav_throttle();
}
void

261
ArduCopterMega/Attitude.pde
View File

@ -13,11 +13,6 @@ init_pids()
void
control_nav_mixer()
{
// limit the nav pitch and roll of the copter
long pmax = g.pitch_max.get();
nav_roll = constrain(nav_roll, -pmax, pmax);
nav_pitch = constrain(nav_pitch, -pmax, pmax);
// control +- 45° is mixed with the navigation request by the Autopilot
// output is in degrees = target pitch and roll of copter
g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
@ -33,6 +28,15 @@ simple_mixer()
g.rc_2.servo_out = nav_pitch;
}
void
limit_nav_pitch_roll(long pmax)
{
// limit the nav pitch and roll of the copter
//long pmax = g.pitch_max.get();
nav_roll = constrain(nav_roll, -pmax, pmax);
nav_pitch = constrain(nav_pitch, -pmax, pmax);
}
void
output_stabilize_roll()
{
@ -95,6 +99,130 @@ output_stabilize_pitch()
g.rc_2.servo_out -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
}
void
output_rate_roll()
{
// rate control
long rate = degrees(omega.x) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)g.rc_1.control_in * 8) - rate; // control is += 4500 * 8 = 36000
g.rc_1.servo_out = g.pid_acro_rate_roll.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
g.rc_1.servo_out = constrain(g.rc_1.servo_out, -2400, 2400); // limit to 2400
}
void
output_rate_pitch()
{
// rate control
long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)g.rc_2.control_in * 8) - rate; // control is += 4500 * 8 = 36000
g.rc_2.servo_out = g.pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
g.rc_2.servo_out = constrain(g.rc_2.servo_out, -2400, 2400); // limit to 2400
}
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
void
reset_I(void)
{
// I removed these, they don't seem to be needed.
//g.pid_nav_lat.reset_I();
//g.pid_nav_lon.reset_I();
}
/*************************************************************
throttle control
****************************************************************/
// user input:
// -----------
void output_manual_throttle()
{
g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost();
}
// Autopilot
// ---------
void output_auto_throttle()
{
g.rc_3.servo_out = (float)nav_throttle * angle_boost();
// make sure we never send a 0 throttle that will cut the motors
g.rc_3.servo_out = max(g.rc_3.servo_out, 1);
}
void calc_nav_throttle()
{
// limit error
long error = constrain(altitude_error, -400, 400);
float scaler = 1.0;
if(error < 0){
// try and prevent rapid fall
scaler = (altitude_sensor == BARO) ? .9 : .9;
}
if(altitude_sensor == BARO){
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler); // .25
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 120);
}else{
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler); // .5
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 150);
}
nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
nav_throttle_old = nav_throttle;
invalid_throttle = false;
//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
}
float angle_boost()
{
float temp = cos_pitch_x * cos_roll_x;
temp = 2.0 - constrain(temp, .5, 1.0);
return temp;
}
/*************************************************************
yaw control
****************************************************************/
void output_manual_yaw()
{
if(g.rc_3.control_in == 0){
// we want to only call this once
if(did_clear_yaw_control == false){
clear_yaw_control();
did_clear_yaw_control = true;
}
}else{ // motors running
// Yaw control
if(g.rc_4.control_in == 0){
output_yaw_with_hold(true); // hold yaw
}else{
output_yaw_with_hold(false); // rate control yaw
}
did_clear_yaw_control = false;
}
}
void auto_yaw()
{
if(yaw_tracking & TRACK_NEXT_WP){
nav_yaw = target_bearing;
}
output_yaw_with_hold(true); // hold yaw
}
void
clear_yaw_control()
{
@ -175,126 +303,3 @@ output_yaw_with_hold(boolean hold)
//Serial.printf("%d\n",g.rc_4.servo_out);
}
void
output_rate_roll()
{
// rate control
long rate = degrees(omega.x) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)g.rc_1.control_in * 8) - rate; // control is += 4500 * 8 = 36000
g.rc_1.servo_out = g.pid_acro_rate_roll.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
g.rc_1.servo_out = constrain(g.rc_1.servo_out, -2400, 2400); // limit to 2400
}
void
output_rate_pitch()
{
// rate control
long rate = degrees(omega.y) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun!
long error = ((long)g.rc_2.control_in * 8) - rate; // control is += 4500 * 8 = 36000
g.rc_2.servo_out = g.pid_acro_rate_pitch.get_pid(error, delta_ms_fast_loop, 1.0); // .075 * 36000 = 2700
g.rc_2.servo_out = constrain(g.rc_2.servo_out, -2400, 2400); // limit to 2400
}
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
void
reset_I(void)
{
g.pid_nav_lat.reset_I();
g.pid_nav_lon.reset_I();
g.pid_baro_throttle.reset_I();
g.pid_sonar_throttle.reset_I();
}
/*************************************************************
throttle control
****************************************************************/
// user input:
// -----------
void output_manual_throttle()
{
g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost();
}
// Autopilot
// ---------
void output_auto_throttle()
{
g.rc_3.servo_out = (float)nav_throttle * angle_boost();
// make sure we never send a 0 throttle that will cut the motors
g.rc_3.servo_out = max(g.rc_3.servo_out, 1);
}
void calc_nav_throttle()
{
// limit error
long error = constrain(altitude_error, -400, 400);
float scaler = 1.0;
if(error < 0){
// try and prevent rapid fall
scaler = (altitude_sensor == BARO) ? .3 : .3;
}
if(altitude_sensor == BARO){
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_fast_loop, scaler); // .25
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 120);
}else{
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, scaler); // .5
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -50, 150);
}
nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
nav_throttle_old = nav_throttle;
//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
}
float angle_boost()
{
float temp = cos_pitch_x * cos_roll_x;
temp = 2.0 - constrain(temp, .5, 1.0);
return temp;
}
/*************************************************************
yaw control
****************************************************************/
void output_manual_yaw()
{
if(g.rc_3.control_in == 0){
// we want to only call this once
if(did_clear_yaw_control == false){
clear_yaw_control();
did_clear_yaw_control = true;
}
}else{ // motors running
// Yaw control
if(g.rc_4.control_in == 0){
output_yaw_with_hold(true); // hold yaw
}else{
output_yaw_with_hold(false); // rate control yaw
}
did_clear_yaw_control = false;
}
}
void auto_yaw()
{
if(yaw_tracking & TRACK_NEXT_WP){
nav_yaw = target_bearing;
}
output_yaw_with_hold(true); // hold yaw
}

9
ArduCopterMega/GCS_Mavlink.pde
View File

@ -363,7 +363,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
// send waypoint
tell_command = get_wp_with_index(packet.seq);
tell_command = get_command_with_index(packet.seq);
// set frame of waypoint
uint8_t frame;
@ -507,7 +507,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
{
Location temp; // XXX this is gross
temp = get_wp_with_index(packet.seq);
temp = get_command_with_index(packet.seq);
set_next_WP(&temp);
}
@ -631,7 +631,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
set_wp_with_index(tell_command, packet.seq);
set_command_with_index(tell_command, packet.seq);
// update waypoint receiving state machine
waypoint_timelast_receive = millis();
@ -939,4 +939,5 @@ GCS_MAVLINK::_queued_send()
}
#endif

2
ArduCopterMega/GCS_Standard.pde
View File

@ -202,7 +202,7 @@ void send_message(byte id, long param) {
tempint = g.waypoint_total; // list length (# of commands in mission)
mess_buffer[5] = tempint & 0xff;
mess_buffer[6] = (tempint >> 8) & 0xff;
tell_command = get_wp_with_index((int)param);
tell_command = get_command_with_index((int)param);
mess_buffer[7] = tell_command.id; // command id
mess_buffer[8] = tell_command.p1; // P1
tempint = tell_command.alt; // P2

6
ArduCopterMega/GCS_Xplane.pde
View File

@ -34,7 +34,7 @@ void send_message(byte id, long param) {
break;
case MSG_COMMAND:
struct Location cmd = get_wp_with_index(param);
struct Location cmd = get_command_with_index(param);
print_waypoint(&cmd, param);
break;
@ -113,12 +113,12 @@ void print_waypoints()
// create a location struct to hold the temp Waypoints for printing
//Location tmp;
Serial.println("Home: ");
struct Location cmd = get_wp_with_index(0);
struct Location cmd = get_command_with_index(0);
print_waypoint(&cmd, 0);
Serial.println("Commands: ");
for (int i=1; i < g.waypoint_total; i++){
cmd = get_wp_with_index(i);
cmd = get_command_with_index(i);
print_waypoint(&cmd, i);
}
}

11
ArduCopterMega/Log.pde
View File

@ -385,13 +385,16 @@ void Log_Write_Nav_Tuning()
DataFlash.WriteInt((int)sonar_alt); // 7
DataFlash.WriteInt((int)baro_alt); // 8
DataFlash.WriteInt(home.alt); // 9
DataFlash.WriteInt((int)next_WP.alt); // 11
DataFlash.WriteInt((int)home.alt); // 9
DataFlash.WriteInt((int)next_WP.alt); // 10
DataFlash.WriteInt((int)altitude_error); // 11
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
@ -446,8 +449,8 @@ void Log_Write_GPS()
// Read a GPS packet
void Log_Read_GPS()
{ // 1 2 3 4 5 6 7 8 9
// GPS, t, 1, 8, 37.7659070, -122.4329400, 57.0500, 58.1400, 658.8400, -11636846.0000
{ // 1 2 3 4 5 6 7 8 9
//GPS, 77361250, 1, 9, 40.0584750, -105.2034500, 166.2600, 2.8100, 0.0600, 266.0000
// 1 2 3 4 5 6 7 8 9
Serial.printf_P(PSTR("GPS, %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"),

13
ArduCopterMega/Parameters.h
View File

@ -17,7 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 4;
static const uint16_t k_format_version = 5;
//
// Parameter identities.
@ -117,6 +117,7 @@ public:
k_param_pid_yaw,
k_param_pid_nav_lat,
k_param_pid_nav_lon,
k_param_pid_nav_wp,
k_param_pid_baro_throttle,
k_param_pid_sonar_throttle,
// special D term alternatives
@ -202,6 +203,7 @@ public:
PID pid_yaw;
PID pid_nav_lat;
PID pid_nav_lon;
PID pid_nav_wp;
PID pid_baro_throttle;
PID pid_sonar_throttle;
@ -272,11 +274,12 @@ public:
pid_stabilize_pitch (k_param_pid_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, 0, STABILIZE_PITCH_IMAX * 100),
pid_yaw (k_param_pid_yaw, PSTR("STB_YAW_"), YAW_P, YAW_I, 0, YAW_IMAX * 100),
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100),
pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100),
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100),
pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100),
pid_nav_wp (k_param_pid_nav_wp, PSTR("NAV_WP_"), NAV_WP_P, NAV_WP_I, NAV_WP_D, NAV_WP_IMAX * 100),
pid_baro_throttle (k_param_pid_baro_throttle, PSTR("THR_BAR_"), THROTTLE_BARO_P, THROTTLE_BARO_I, THROTTLE_BARO_D, THROTTLE_BARO_IMAX * 100),
pid_sonar_throttle (k_param_pid_sonar_throttle, PSTR("THR_SON_"), THROTTLE_SONAR_P, THROTTLE_SONAR_I, THROTTLE_SONAR_D, THROTTLE_SONAR_IMAX * 100),
pid_baro_throttle (k_param_pid_baro_throttle, PSTR("THR_BAR_"), THROTTLE_BARO_P, THROTTLE_BARO_I, THROTTLE_BARO_D, THROTTLE_BARO_IMAX),
pid_sonar_throttle (k_param_pid_sonar_throttle, PSTR("THR_SON_"), THROTTLE_SONAR_P, THROTTLE_SONAR_I, THROTTLE_SONAR_D, THROTTLE_SONAR_IMAX),
stabilize_dampener (STABILIZE_ROLL_D, k_param_stabilize_dampener, PSTR("STB_DAMP")),
hold_yaw_dampener (YAW_D, k_param_hold_yaw_dampener, PSTR("YAW_DAMP")),

16
ArduCopterMega/command description.txt
View File

@ -16,6 +16,9 @@ Command Structure in bytes
11 0x0C ..
11 0x0D ..
Update:
MAV_CMD_NAV_ORIENTATION_TARGET > MAV_CMD_DO_ORIENTATION
Remove MAV_CMD_NAV_TARGET
***********************************
@ -28,7 +31,6 @@ Command ID Name Parameter 1 Altitude Latitude Longitude
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - - - -
0x15 / 21 MAV_CMD_NAV_LAND - - - -
0x16 / 22 MAV_CMD_NAV_TAKEOFF - altitude - -
0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.
@ -37,14 +39,12 @@ Command ID Name Parameter 1 Altitude Latitude Longitude
May Commands - these commands are optional to finish
Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
0x70 / 112 MAV_CMD_CONDITION_DELAY - - time (seconds) -
0x71 / 113 MAV_CMD_CONDITION_CHANGE_ALT rate (cm/sec) alt (finish) - -
Note: rate must be > 10 cm/sec due to integer math
MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM)
// MAV_CMD_NAV_LAND_OPTIONS (NOT CURRENTLY IN MAVLINK PROTOCOL OR IMPLEMENTED IN APM)
0x72 / 114 MAV_CMD_CONDITION_DISTANCE - - distance (meters) -
0x71 / 115 MAV_CMD_CONDITION_YAW angle speed direction (-1,1) rel (1), abs (0)
***********************************
@ -65,14 +65,16 @@ Command ID Name Parameter 1 Parameter 2 Parameter 3 Parameter 4
(1=use current location, 0=use specified location)
0xB4 / 180 MAV_CMD_DO_SET_PARAMETER Param number Param value (NOT CURRENTLY IMPLEMENTED IN APM)
0xB5 / 181 MAV_CMD_DO_SET_RELAY Relay number On/off (1/0) - -
0xB6 / 182 MAV_CMD_DO_REPEAT_RELAY Relay number Cycle count Cycle time (sec) -
Note: Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
0xB7 / 183 MAV_CMD_DO_SET_SERVO Servo number (5-8) On/off (1/0) - -
0xB6 / 184 MAV_CMD_DO_REPEAT_SERVO Servo number (5-8) Cycle count Cycle time (sec) -
Note: Max cycle time = 60 sec, A repeat relay or repeat servo command will cancel any current repeating event
// need to add command
0xB7 / 185 MAV_CMD_DO_ORIENTATION Yaw_Mode altitude lat lon
TRACK_NONE 1
TRACK_NEXT_WP 2
TRACK_TARGET_WP 4

91
ArduCopterMega/commands.pde
View File

@ -2,11 +2,15 @@
void init_commands()
{
//read_EEPROM_waypoint_info();
g.waypoint_index.set_and_save(0);
command_must_index = 0;
command_may_index = 0;
next_command.id = CMD_BLANK;
// This are registers for the current may and must commands
// setting to zero will allow them to be written to by new commands
command_must_index = NO_COMMAND;
command_may_index = NO_COMMAND;
// clear the command queue
clear_command_queue();
}
void init_auto()
@ -21,29 +25,33 @@ void init_auto()
init_commands();
}
// this is only used by an air-start
/*void reload_commands_airstart()
{
init_commands();
g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all?
decrement_WP_index();
// forces the loading of a new command
// queue is emptied after a new command is processed
void clear_command_queue(){
next_command.id = CMD_BLANK;
}
*/
// Getters
// -------
struct Location get_wp_with_index(int i)
struct Location get_command_with_index(int i)
{
struct Location temp;
long mem;
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.waypoint_total) {
// we do not have a valid command to load
// return a WP with a "Blank" id
temp.id = CMD_BLANK;
// no reason to carry on
return temp;
}else{
// we can load a command, we don't process it yet
// read WP position
mem = (WP_START_BYTE) + (i * WP_SIZE);
long mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = eeprom_read_byte((uint8_t*)mem);
mem++;
@ -62,30 +70,18 @@ struct Location get_wp_with_index(int i)
temp.lng = (long)eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
}
// Add on home altitude if we are a nav command
if(temp.id < 0x70){
temp.alt += home.alt;
}
if(temp.options & WP_OPTION_RELATIVE){
// If were relative, just offset from home
temp.lat += home.lat;
temp.lng += home.lng;
}
// XXX this is a little awkward. We have two methods to control Yaw tracking
// one is global and one is per waypoint.
if(temp.options & WP_OPTION_YAW){
yaw_tracking = TRACK_NEXT_WP;
}
// this is a hack for now, until we get GUI support
yaw_tracking = TRACK_NEXT_WP;
return temp;
}
// Setters
// -------
void set_wp_with_index(struct Location temp, int i)
void set_command_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.waypoint_total.get());
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
@ -99,13 +95,13 @@ void set_wp_with_index(struct Location temp, int i)
eeprom_write_byte((uint8_t *) mem, temp.p1);
mem++;
eeprom_write_dword((uint32_t *) mem, temp.alt); // alt is stored in CM!
eeprom_write_dword((uint32_t *) mem, temp.alt); // Alt is stored in CM!
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lat);
eeprom_write_dword((uint32_t *) mem, temp.lat); // Lat is stored in decimal degrees * 10^7
mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lng);
eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7
}
void increment_WP_index()
@ -137,7 +133,8 @@ long read_alt_to_hold()
//********************************************************************************
//This function sets the waypoint and modes for Return to Launch
// This function sets the waypoint and modes for Return to Launch
// It's not currently used
//********************************************************************************
Location get_LOITER_home_wp()
@ -146,19 +143,19 @@ Location get_LOITER_home_wp()
next_WP = current_loc;
// read home position
struct Location temp = get_wp_with_index(0);
struct Location temp = get_command_with_index(0); // 0 = home
temp.id = MAV_CMD_NAV_LOITER_UNLIM;
temp.alt = read_alt_to_hold();
return temp;
}
/*
This function stores waypoint commands
It looks to see what the next command type is and finds the last command.
This function sets the next waypoint command
It precalculates all the necessary stuff.
*/
void set_next_WP(struct Location *wp)
{
//GCS.send_text_P(SEVERITY_LOW,PSTR("load WP"));
SendDebug("MSG <set_next_wp> wp_index: ");
SendDebugln(g.waypoint_index, DEC);
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
@ -171,14 +168,14 @@ void set_next_WP(struct Location *wp)
// ---------------------
next_WP = *wp;
// used to control and limit the rate of climb
// -----------------------------------------------
// used to control and limit the rate of climb - not used right now!
// -----------------------------------------------------------------
target_altitude = current_loc.alt;
// zero out our loiter vals to watch for missed waypoints
loiter_delta = 0;
loiter_sum = 0;
loiter_total = 0;
//loiter_delta = 0;
//loiter_sum = 0;
//loiter_total = 0;
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
@ -187,13 +184,15 @@ void set_next_WP(struct Location *wp)
// this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP);
// this makes the data not log for a GPS read
wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP);
nav_bearing = target_bearing;
// to check if we have missed the WP
// ----------------------------
old_target_bearing = target_bearing;
saved_target_bearing = target_bearing;
// set a new crosstrack bearing
// ----------------------------
@ -217,19 +216,17 @@ void init_home()
home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7
home.alt = max(g_gps->altitude, 0);
home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid
home_is_set = true;
//Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt);
// Save Home to EEPROM
// -------------------
set_wp_with_index(home, 0);
set_command_with_index(home, 0);
print_wp(&home, 0);
// Save prev loc
// -------------
// Save prev loc this makes the calcs look better before commands are loaded
next_WP = prev_WP = home;
}

103
ArduCopterMega/commands_logic.pde
View File

@ -5,10 +5,6 @@
/********************************************************************************/
void handle_process_must()
{
// reset navigation integrators
// -------------------------
reset_I();
switch(next_command.id){
case MAV_CMD_NAV_TAKEOFF:
@ -101,23 +97,27 @@ void handle_process_now()
do_repeat_relay();
break;
case MAV_CMD_NAV_ORIENTATION_TARGET:
do_target_yaw();
//case MAV_CMD_DO_ORIENTATION:
// do_target_yaw();
}
}
bool handle_no_commands()
void handle_no_commands()
{
// we don't want to RTL. Maybe this will change in the future. RTL is kinda dangerous.
// use landing commands
return;
/*
if (command_must_ID)
return false;
return;
switch (control_mode){
default:
//set_mode(RTL);
break;
}
return true;
return;
*/
}
/********************************************************************************/
@ -219,7 +219,9 @@ void do_RTL(void)
void do_takeoff()
{
Location temp = current_loc;
temp.alt = next_command.alt;
// next_command.alt is a relative altitude!!!
temp.alt = next_command.alt + home.alt;
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
@ -231,11 +233,21 @@ void do_takeoff()
void do_nav_wp()
{
set_next_WP(&next_command);
wp_verify_byte = 0;
loiter_time = 0;
loiter_time_max = next_command.p1 * 1000; // units are (seconds)
// next_command.alt is a relative altitude!!!
next_command.alt += home.alt;
set_next_WP(&next_command);
// this is our bitmask to verify we have met all conditions to move on
wp_verify_byte = 0;
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0;
// this is the delay, stored in seconds and expanded to millis
loiter_time_max = next_command.p1 * 1000;
// if we don't require an altitude minimum, we save this flag as passed (1)
if((next_WP.options & WP_OPTION_ALT_REQUIRED) == 0){
// we don't need to worry about it
wp_verify_byte |= NAV_ALTITUDE;
@ -245,16 +257,20 @@ void do_nav_wp()
void do_land()
{
Serial.println("dlnd ");
land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
// not really used right now, might be good for debugging
land_complete = false;
// A value that drives to 0 when the altitude doesn't change
velocity_land = 2000;
// used to limit decent rate
land_start = millis();
// used to limit decent rate
original_alt = current_loc.alt;
//Location temp = current_loc;
//temp.alt = home.alt;
// just go down far
//temp.alt = -100000;
// hold at our current location
set_next_WP(&current_loc);
}
@ -269,16 +285,19 @@ void do_loiter_unlimited()
void do_loiter_turns()
{
/*
if(next_command.lat == 0)
set_next_WP(&current_loc);
else
set_next_WP(&next_command);
loiter_total = next_command.p1 * 360;
*/
}
void do_loiter_time()
{
/*
if(next_command.lat == 0)
set_next_WP(&current_loc);
else
@ -287,6 +306,7 @@ void do_loiter_time()
loiter_time = millis();
loiter_time_max = next_command.p1 * 1000; // units are (seconds)
Serial.printf("dlt %ld, max %ld\n",loiter_time, loiter_time_max);
*/
}
/********************************************************************************/
@ -295,7 +315,7 @@ void do_loiter_time()
bool verify_takeoff()
{
//Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt);
Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt);
// wait until we are ready!
if(g.rc_3.control_in == 0)
@ -305,7 +325,9 @@ bool verify_takeoff()
Serial.println("Y");
takeoff_complete = true;
return true;
}else{
Serial.println("N");
return false;
}
@ -314,7 +336,7 @@ bool verify_takeoff()
bool verify_land()
{
// land at 1 meter per second
next_WP.alt = original_alt - ((millis() - land_start) / 10); // condition_value = our initial
next_WP.alt = original_alt - ((millis() - land_start) / 5); // condition_value = our initial
velocity_land = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8);
old_alt = current_loc.alt;
@ -324,15 +346,16 @@ bool verify_land()
if(sonar_alt < 40){
land_complete = true;
Serial.println("Y");
return true;
//return true;
}
}
if(velocity_land <= 0){
land_complete = true;
return true;
//return true;
}
Serial.printf("N, %d\n", velocity_land);
//Serial.printf("N, %d\n", velocity_land);
Serial.printf("N_alt, %d\n", next_WP.alt);
//update_crosstrack();
return false;
@ -351,16 +374,24 @@ bool verify_nav_wp()
}
}
// Distance checking
if((wp_distance > 0) && (wp_distance <= g.waypoint_radius)){
wp_verify_byte |= NAV_LOCATION;
if(loiter_time == 0){
loiter_time = millis();
// Did we pass the WP? // Distance checking
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
// if we have a distance calc error, wp_distance may be less than 0
if(wp_distance > 0){
// XXX does this work?
wp_verify_byte |= NAV_LOCATION;
if(loiter_time == 0){
loiter_time = millis();
}
}
}
// Hold at Waypoint checking
if(wp_verify_byte & NAV_LOCATION){ // we have reached our goal
// Hold at Waypoint checking, we cant move on until this is OK
if(wp_verify_byte & NAV_LOCATION){
// we have reached our goal
if ((millis() - loiter_time) > loiter_time_max) {
wp_verify_byte |= NAV_DELAY;
@ -371,7 +402,7 @@ bool verify_nav_wp()
if(wp_verify_byte == 7){
char message[30];
sprintf(message,"Reached Waypoint #%i",command_must_index);
sprintf(message,"Reached Command #%i",command_must_index);
gcs.send_text(SEVERITY_LOW,message);
return true;
}else{
@ -579,10 +610,10 @@ void do_jump()
command_must_index = 0;
command_may_index = 0;
temp = get_wp_with_index(g.waypoint_index);
temp = get_command_with_index(g.waypoint_index);
temp.lat = next_command.lat - 1; // Decrement repeat counter
set_wp_with_index(temp, g.waypoint_index);
set_command_with_index(temp, g.waypoint_index);
g.waypoint_index.set_and_save(next_command.p1 - 1);
}
}

116
ArduCopterMega/commands_process.pde
View File

@ -6,17 +6,41 @@ void update_commands(void)
{
// This function loads commands into three buffers
// when a new command is loaded, it is processed with process_XXX()
// ----------------------------------------------------------------
if(home_is_set == false){
return; // don't do commands
// If we have a command in the queue,
// nothing to do.
if(next_command.id != NO_COMMAND){
return;
}
if(control_mode == AUTO){
load_next_command_from_EEPROM();
// fetch next command if the next command queue is empty
// -----------------------------------------------------
next_command = get_command_with_index(g.waypoint_index + 1);
if(next_command.id == NO_COMMAND){
// if no commands were available from EEPROM
// --------------------------------------------
handle_no_commands();
gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
} else {
// A command was loaded from EEPROM
// --------------------------------------------
// debug by outputing the Waypoint loaded
print_wp(&next_command, g.waypoint_index + 1);
// Set our current mission index + 1;
increment_WP_index();
// act on our new command
process_next_command();
} // Other (eg GCS_Auto) modes may be implemented here
}
}
// called with GPS navigation update - not constantly
void verify_commands(void)
{
if(verify_must()){
@ -29,44 +53,27 @@ void verify_commands(void)
}
}
void load_next_command_from_EEPROM()
{
// fetch next command if the next command queue is empty
// -----------------------------------------------------
if(next_command.id == NO_COMMAND){
next_command = get_wp_with_index(g.waypoint_index + 1);
//Serial.println("AA");
print_wp(&next_command, g.waypoint_index + 1);
}
// If the preload failed, return or just Loiter
// --------------------------------------------
if(next_command.id == NO_COMMAND){
Serial.println("lnc_nc");
// we are out of commands!
if(handle_no_commands() == true){
gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
}
}
}
void process_next_command()
{
// these are Navigation/Must commands
// ---------------------------------
if (command_must_index == 0){ // no current command loaded
if (command_must_index == NO_COMMAND){ // no current command loaded
if (next_command.id < MAV_CMD_NAV_LAST ){
increment_WP_index();
//save_command_index(); // TO DO - fix - to Recover from in air Restart
// we remember the index of our mission here
command_must_index = g.waypoint_index;
// dubugging output
SendDebug("MSG <pnc> new c_must_id ");
SendDebug(next_command.id,DEC);
SendDebug(" index:");
SendDebugln(command_must_index,DEC);
// Save CMD to Log
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
// Act on the new command
process_must();
}
}
@ -75,23 +82,26 @@ void process_next_command()
// ----------------------
if (command_may_index == 0){
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
// we remember the index of our mission here
command_may_index = g.waypoint_index;
SendDebug("MSG <pnc> new may ");
SendDebugln(next_command.id,DEC);
//Serial.print("new command_may_index ");
//Serial.println(command_may_index,DEC);
SendDebug("MSG <pnc> new may ");
SendDebugln(next_command.id,DEC);
//Serial.print("new command_may_index ");
//Serial.println(command_may_index,DEC);
// Save CMD to Log
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
process_may();
}
// these are Do/Now commands
// ---------------------------
if (next_command.id > MAV_CMD_CONDITION_LAST){
increment_WP_index(); // this command is from the command list in EEPROM
SendDebug("MSG <pnc> new now ");
SendDebugln(next_command.id,DEC);
SendDebug("MSG <pnc> new now ");
SendDebugln(next_command.id,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);
@ -108,18 +118,21 @@ void process_must()
//gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
//Serial.printf("pmst %d\n", (int)next_command.id);
print_wp(&next_command, g.waypoint_index);
// clear May indexes
// clear May indexes to force loading of more commands
// existing May commands are tossed.
command_may_index = NO_COMMAND;
command_may_ID = NO_COMMAND;
// remember our command ID
command_must_ID = next_command.id;
// implements the Flight Logic
handle_process_must();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
}
void process_may()
@ -131,9 +144,9 @@ void process_may()
command_may_ID = next_command.id;
handle_process_may();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
}
void process_now()
@ -141,11 +154,8 @@ void process_now()
Serial.print("pnow");
handle_process_now();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
//gcs.send_text_P(SEVERITY_LOW, PSTR("<process_now>"));
//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
}

44
ArduCopterMega/config.h
View File

@ -360,24 +360,38 @@
//
// how much to we pitch towards the target
#ifndef PITCH_MAX
# define PITCH_MAX 15 // degrees
# define PITCH_MAX 45 // degrees
#endif
//////////////////////////////////////////////////////////////////////////////
// Navigation control gains
//
#ifndef NAV_P
# define NAV_P 2.0
#ifndef NAV_LOITER_P
# define NAV_LOITER_P 2.0
#endif
#ifndef NAV_I
# define NAV_I 0.1
#ifndef NAV_LOITER_I
# define NAV_LOITER_I 0.1
#endif
#ifndef NAV_D
# define NAV_D 0.00 // should always be 0
#ifndef NAV_LOITER_D
# define NAV_LOITER_D 0.00
#endif
#ifndef NAV_IMAX
# define NAV_IMAX 250 // 250 Lat and Longtitude
#ifndef NAV_LOITER_IMAX
# define NAV_LOITER_IMAX 250 // 250 Lat and Longtitude
#endif
#ifndef NAV_WP_P
# define NAV_WP_P 4.0
#endif
#ifndef NAV_WP_I
# define NAV_WP_I 0.0
#endif
#ifndef NAV_WP_D
# define NAV_WP_D 15
#endif
#ifndef NAV_WP_IMAX
# define NAV_WP_IMAX 20 // 20 degrees
#endif
@ -391,24 +405,24 @@
# define THROTTLE_BARO_I 0.1
#endif
#ifndef THROTTLE_BARO_D
# define THROTTLE_BARO_D 0.03 // lowered to 0 to debug effects
# define THROTTLE_BARO_D 0.1
#endif
#ifndef THROTTLE_BARO_IMAX
# define THROTTLE_BARO_IMAX 50
# define THROTTLE_BARO_IMAX 80
#endif
#ifndef THROTTLE_SONAR_P
# define THROTTLE_SONAR_P .8 // upped a hair from .5
# define THROTTLE_SONAR_P .8 // upped from .5
#endif
#ifndef THROTTLE_SONAR_I
# define THROTTLE_SONAR_I 0.4
# define THROTTLE_SONAR_I 0.1
#endif
#ifndef THROTTLE_SONAR_D
# define THROTTLE_SONAR_D 0.15
# define THROTTLE_SONAR_D 0.1
#endif
#ifndef THROTTLE_SONAR_IMAX
# define THROTTLE_SONAR_IMAX 50
# define THROTTLE_SONAR_IMAX 80
#endif

2
ArduCopterMega/control_modes.pde
View File

@ -14,7 +14,7 @@ void read_control_switch()
// reset navigation integrators
// -------------------------
reset_I();
//reset_I();
}
}

4
ArduCopterMega/motors.pde
View File

@ -319,7 +319,7 @@ set_servos_4()
}else{
// our motor is unarmed, we're on the ground
reset_I();
//reset_I();
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
@ -345,7 +345,7 @@ set_servos_4()
}
// reset I terms of PID controls
reset_I();
//reset_I();
// Initialize yaw command to actual yaw when throttle is down...
g.rc_4.control_in = ToDeg(dcm.yaw);

107
ArduCopterMega/navigation.pde
View File

@ -35,16 +35,13 @@ void navigate()
// ------------------------------------------
nav_bearing = target_bearing;
// check if we have missed the WP
loiter_delta = (target_bearing - old_target_bearing)/100;
}
// reset the old value
old_target_bearing = target_bearing;
// wrap values
if (loiter_delta > 180) loiter_delta -= 360;
if (loiter_delta < -180) loiter_delta += 360;
loiter_sum += abs(loiter_delta);
bool check_missed_wp()
{
long temp = target_bearing - saved_target_bearing;
temp = wrap_180(temp);
return (abs(temp) > 10000); //we pased the waypoint by 10 °
}
#define DIST_ERROR_MAX 1800
@ -59,6 +56,7 @@ void calc_loiter_nav()
10000 = 111m
pitch_max = 22° (2200)
*/
// X ROLL
long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 500 - 0 = 500 roll EAST
@ -77,10 +75,6 @@ void calc_loiter_nav()
//nav_lat = lat_error * g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0); // invert lat (for pitch)
// nav_lat = -1000 Y Pitch
// nav_lon = 1000 X Roll
// rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * -cos_yaw_x;
// BAD
@ -110,13 +104,37 @@ void calc_loiter_nav()
}
void calc_waypoint_nav()
void calc_simple_nav()
{
// no dampening here in SIMPLE mode
nav_lat = constrain((wp_distance * 100), -1800, 1800); // +- 20m max error
// Scale response by kP
nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
}
void calc_rate_nav()
{
// calc distance error
nav_lat = constrain((wp_distance * 100), -1800, 1800); // +- 20m max error
// Scale response by kP
nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
// Scale response by kP
//long output = g.pid_nav_wp.kP() * error;
int dampening = g.pid_nav_wp.kD() * (g_gps->ground_speed - last_ground_speed);
// remember our old speed
last_ground_speed = g_gps->ground_speed;
// dampen our response
nav_lat -= constrain(dampening, -1800, 1800); // +- 20m max error
}
void calc_nav_output()
{
// get the sin and cos of the bearing error - rotated 90°
sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100));
@ -124,9 +142,40 @@ void calc_waypoint_nav()
// rotate the vector
nav_roll = (float)nav_lat * cos_nav_x;
nav_pitch = -(float)nav_lat * sin_nav_y;
}
#define WAYPOINT_SPEED 450
// called after we get GPS read
void calc_rate_nav2()
{
// change to rate error
// we want to be going 450cm/s
int error = WAYPOINT_SPEED - g_gps->ground_speed;
// which direction are we moving?
long target_error = target_bearing - g_gps->ground_course;
target_error = wrap_180(target_error);
// calc the cos of the error to tell how fast we are moving towards the target
error = (float)error * cos(radians((float)target_error/100));
// Scale response by kP
long nav_lat = g.pid_nav_wp.kP() * error;
int dampening = g.pid_nav_wp.kD() * (g_gps->ground_speed - last_ground_speed);
// remember our old speed
last_ground_speed = g_gps->ground_speed;
// dampen our response
nav_lat -= constrain(dampening, -1800, 1800); // +- 20m max error
// limit our output
nav_lat = constrain(nav_lat, -3800, 3800); // +- 20m max error
}
void calc_bearing_error()
{
bearing_error = nav_bearing - dcm.yaw_sensor;
@ -167,34 +216,6 @@ long wrap_180(long error)
return error;
}
void update_loiter()
{
float power;
if(wp_distance <= g.loiter_radius){
power = float(wp_distance) / float(g.loiter_radius);
nav_bearing += (int)(9000.0 * (2.0 + power));
}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE);
power = constrain(power, 0, 1);
nav_bearing -= power * 9000;
}else{
update_crosstrack();
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
}
if (wp_distance < g.loiter_radius){
nav_bearing += 9000;
}else{
nav_bearing -= 100 * M_PI / 180 * asin(g.loiter_radius / wp_distance);
}
update_crosstrack;
nav_bearing = wrap_360(nav_bearing);
}
void update_crosstrack(void)
{
// Crosstrack Error

5
ArduCopterMega/read_me.text
View File

@ -78,3 +78,8 @@ Auto modes will NOT engage until the throttle is above neutral. So if you put th
Good luck,
Jason
TODO:
Move crosstrack navigation to full PID to limit the velocity towards a waypoint

9
ArduCopterMega/setup.pde
View File

@ -700,6 +700,7 @@ void default_log_bitmask()
void
default_gains()
{
/*
// acro, angular rate
g.pid_acro_rate_roll.kP(ACRO_RATE_ROLL_P);
g.pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I);
@ -764,6 +765,7 @@ default_gains()
g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX);
save_EEPROM_PID();
*/
}
@ -775,11 +777,11 @@ void report_wp(byte index = 255)
{
if(index == 255){
for(byte i = 0; i <= g.waypoint_total; i++){
struct Location temp = get_wp_with_index(i);
struct Location temp = get_command_with_index(i);
print_wp(&temp, i);
}
}else{
struct Location temp = get_wp_with_index(index);
struct Location temp = get_command_with_index(index);
print_wp(&temp, index);
}
}
@ -1185,7 +1187,7 @@ void read_EEPROM_PID(void)
void save_EEPROM_PID(void)
{
/*
g.pid_acro_rate_roll.save_gains();
g.pid_acro_rate_pitch.save_gains();
g.pid_acro_rate_yaw.save_gains();
@ -1203,6 +1205,7 @@ void save_EEPROM_PID(void)
g.stabilize_dampener.save();
// yaw
g.hold_yaw_dampener.save();
*/
}
/********************************************************************************/

16
ArduCopterMega/test.pde
View File

@ -828,38 +828,38 @@ test_mission(uint8_t argc, const Menu::arg *argv)
// clear home
{Location t = {0, 0, 0, 0, 0, 0};
set_wp_with_index(t,0);}
set_command_with_index(t,0);}
// CMD opt pitch alt/cm
{Location t = {MAV_CMD_NAV_TAKEOFF, 0, 0, 100, 0, 0};
set_wp_with_index(t,1);}
set_command_with_index(t,1);}
if (!strcmp_P(argv[1].str, PSTR("wp"))) {
// CMD opt
{Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0};
set_wp_with_index(t,2);}
set_command_with_index(t,2);}
// CMD opt
{Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0};
set_wp_with_index(t,3);}
set_command_with_index(t,3);}
// CMD opt
{Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
set_wp_with_index(t,4);}
set_command_with_index(t,4);}
} else {
//2250 = 25 meteres
// CMD opt p1 //alt //NS //WE
{Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 0, 100, 1100, 0};
set_wp_with_index(t,2);}
set_command_with_index(t,2);}
// CMD opt dir angle/deg deg/s relative
{Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1};
set_wp_with_index(t,3);}
set_command_with_index(t,3);}
// CMD opt
{Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0};
set_wp_with_index(t,4);}
set_command_with_index(t,4);}
}