mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-25 01:03:59 -04:00
Warning!!! The recent build 1919 was tagged as sparkfun.
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
parent
16412abbde
commit
121cf0f658
@ -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
|
||||
|
||||
|
||||
@ -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(¤t_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
|
||||
|
||||
@ -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
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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"),
|
||||
|
||||
|
||||
@ -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,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
|
||||
|
||||
@ -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(¤t_loc, &next_WP);
|
||||
|
||||
// this makes the data not log for a GPS read
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing = get_bearing(¤t_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;
|
||||
}
|
||||
|
||||
|
||||
@ -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(¤t_loc);
|
||||
}
|
||||
|
||||
@ -269,16 +285,19 @@ void do_loiter_unlimited()
|
||||
|
||||
void do_loiter_turns()
|
||||
{
|
||||
/*
|
||||
if(next_command.lat == 0)
|
||||
set_next_WP(¤t_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(¤t_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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
|
||||
@ -14,7 +14,7 @@ void read_control_switch()
|
||||
|
||||
// reset navigation integrators
|
||||
// -------------------------
|
||||
reset_I();
|
||||
//reset_I();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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();
|
||||
*/
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
|
||||
@ -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);}
|
||||
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user