APM: change variables to use _cm, _cd and _ms suffix for units
this makes it less likely that we mix up units
This commit is contained in:
Andrew Tridgell
parent
295a9ce39c
commit
67f076a9db
@ -396,15 +396,19 @@ static bool GPS_enabled = false;
|
||||
// Constants
|
||||
const float radius_of_earth = 6378100; // meters
|
||||
const float gravity = 9.81; // meters/ sec^2
|
||||
|
||||
// This is the currently calculated direction to fly.
|
||||
// deg * 100 : 0 to 360
|
||||
static int32_t nav_bearing;
|
||||
static int32_t nav_bearing_cd;
|
||||
|
||||
// This is the direction to the next waypoint or loiter center
|
||||
// deg * 100 : 0 to 360
|
||||
static int32_t target_bearing;
|
||||
static int32_t target_bearing_cd;
|
||||
|
||||
//This is the direction from the last waypoint to the next waypoint
|
||||
// deg * 100 : 0 to 360
|
||||
static int32_t crosstrack_bearing;
|
||||
static int32_t crosstrack_bearing_cd;
|
||||
|
||||
// Direction held during phases of takeoff and landing
|
||||
// deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use
|
||||
static int32_t hold_course = -1; // deg * 100 dir of plane
|
||||
@ -422,18 +426,23 @@ static byte non_nav_command_ID = NO_COMMAND;
|
||||
// Airspeed
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
|
||||
// Also used for flap deployment criteria. Centimeters per second.static int32_t target_airspeed;
|
||||
static int32_t target_airspeed;
|
||||
// Also used for flap deployment criteria. Centimeters per second.
|
||||
static int32_t target_airspeed_cm;
|
||||
|
||||
// The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second.
|
||||
static float airspeed_error;
|
||||
static float airspeed_error_cm;
|
||||
|
||||
// The calculated total energy error (kinetic (altitude) plus potential (airspeed)).
|
||||
// Used by the throttle controller
|
||||
static int32_t energy_error;
|
||||
|
||||
// kinetic portion of energy error (m^2/s^2)
|
||||
static int32_t airspeed_energy_error;
|
||||
|
||||
// An amount that the airspeed should be increased in auto modes based on the user positioning the
|
||||
// throttle stick in the top half of the range. Centimeters per second.
|
||||
static int16_t airspeed_nudge;
|
||||
static int16_t airspeed_nudge_cm;
|
||||
|
||||
// Similar to airspeed_nudge, but used when no airspeed sensor.
|
||||
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
|
||||
static int16_t throttle_nudge = 0;
|
||||
@ -448,9 +457,11 @@ static int32_t groundspeed_undershoot = 0;
|
||||
// Location Errors
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Difference between current bearing and desired bearing. Hundredths of a degree
|
||||
static int32_t bearing_error;
|
||||
static int32_t bearing_error_cd;
|
||||
|
||||
// Difference between current altitude and desired altitude. Centimeters
|
||||
static int32_t altitude_error;
|
||||
static int32_t altitude_error_cm;
|
||||
|
||||
// Distance perpandicular to the course line that we are off trackline. Meters
|
||||
static float crosstrack_error;
|
||||
|
||||
@ -490,39 +501,47 @@ static bool takeoff_complete = true;
|
||||
static bool land_complete;
|
||||
// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
|
||||
static int32_t takeoff_altitude;
|
||||
|
||||
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
|
||||
static int16_t takeoff_pitch;
|
||||
static int16_t takeoff_pitch_cd;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Loiter management
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Previous target bearing. Used to calculate loiter rotations. Hundredths of a degree
|
||||
static int32_t old_target_bearing;
|
||||
static int32_t old_target_bearing_cd;
|
||||
|
||||
// Total desired rotation in a loiter. Used for Loiter Turns commands. Degrees
|
||||
static int16_t loiter_total;
|
||||
|
||||
// The amount in degrees we have turned since recording old_target_bearing
|
||||
static int16_t loiter_delta;
|
||||
|
||||
// Total rotation in a loiter. Used for Loiter Turns commands and to check for missed waypoints. Degrees
|
||||
static int16_t loiter_sum;
|
||||
|
||||
// The amount of time we have been in a Loiter. Used for the Loiter Time command. Milliseconds.
|
||||
static int32_t loiter_time;
|
||||
static uint32_t loiter_time_ms;
|
||||
|
||||
// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
|
||||
static int16_t loiter_time_max;
|
||||
static uint32_t loiter_time_max_ms;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Navigation control variables
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The instantaneous desired bank angle. Hundredths of a degree
|
||||
static int32_t nav_roll;
|
||||
static int32_t nav_roll_cd;
|
||||
|
||||
// The instantaneous desired pitch angle. Hundredths of a degree
|
||||
static int32_t nav_pitch;
|
||||
static int32_t nav_pitch_cd;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Waypoint distances
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Distance between plane and next waypoint. Meters
|
||||
// is not static because AP_Camera uses it
|
||||
long wp_distance;
|
||||
int32_t wp_distance;
|
||||
|
||||
// Distance between previous and next waypoint. Meters
|
||||
static int32_t wp_totalDistance;
|
||||
|
||||
@ -531,10 +550,13 @@ static int32_t wp_totalDistance;
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Flag indicating current event type
|
||||
static byte event_id;
|
||||
|
||||
// when the event was started in ms
|
||||
static int32_t event_timer;
|
||||
static int32_t event_timer_ms;
|
||||
|
||||
// how long to delay the next firing of event in millis
|
||||
static uint16_t event_delay;
|
||||
static uint16_t event_delay_ms;
|
||||
|
||||
// how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles
|
||||
static int16_t event_repeat = 0;
|
||||
// per command value, such as PWM for servos
|
||||
@ -552,7 +574,7 @@ static int32_t condition_value;
|
||||
// For example in a delay command the condition_start records that start time for the delay
|
||||
static uint32_t condition_start;
|
||||
// A value used in condition commands. For example the rate at which to change altitude.
|
||||
static int16_t condition_rate;
|
||||
static int16_t condition_rate;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// 3D Location vectors
|
||||
@ -579,9 +601,9 @@ static struct Location next_nonnav_command;
|
||||
// Altitude / Climb rate control
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The current desired altitude. Altitude is linearly ramped between waypoints. Centimeters
|
||||
static int32_t target_altitude;
|
||||
static int32_t target_altitude_cm;
|
||||
// Altitude difference between previous and current waypoint. Centimeters
|
||||
static int32_t offset_altitude;
|
||||
static int32_t offset_altitude_cm;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// IMU variables
|
||||
@ -608,16 +630,20 @@ static int16_t pmTest1 = 0;
|
||||
// System Timers
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Time in miliseconds of start of main control loop. Milliseconds
|
||||
static uint32_t fast_loopTimer;
|
||||
static uint32_t fast_loopTimer_ms;
|
||||
|
||||
// Time Stamp when fast loop was complete. Milliseconds
|
||||
static uint32_t fast_loopTimeStamp;
|
||||
static uint32_t fast_loopTimeStamp_ms;
|
||||
|
||||
// Number of milliseconds used in last main loop cycle
|
||||
static uint8_t delta_ms_fast_loop;
|
||||
|
||||
// Counter of main loop executions. Used for performance monitoring and failsafe processing
|
||||
static uint16_t mainLoop_count;
|
||||
|
||||
// Time in miliseconds of start of medium control loop. Milliseconds
|
||||
static uint32_t medium_loopTimer;
|
||||
static uint32_t medium_loopTimer_ms;
|
||||
|
||||
// Counters for branching from main control loop to slower loops
|
||||
static byte medium_loopCounter;
|
||||
// Number of milliseconds used in last medium loop cycle
|
||||
@ -659,11 +685,11 @@ void loop()
|
||||
{
|
||||
// We want this to execute at 50Hz if possible
|
||||
// -------------------------------------------
|
||||
if (millis()-fast_loopTimer > 19) {
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
load = (float)(fast_loopTimeStamp - fast_loopTimer)/delta_ms_fast_loop;
|
||||
if (millis() - fast_loopTimer_ms > 19) {
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer_ms;
|
||||
load = (float)(fast_loopTimeStamp_ms - fast_loopTimer_ms)/delta_ms_fast_loop;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f;
|
||||
fast_loopTimer = millis();
|
||||
fast_loopTimer_ms = millis();
|
||||
|
||||
mainLoop_count++;
|
||||
|
||||
@ -692,7 +718,7 @@ void loop()
|
||||
}
|
||||
}
|
||||
|
||||
fast_loopTimeStamp = millis();
|
||||
fast_loopTimeStamp_ms = millis();
|
||||
}
|
||||
}
|
||||
|
||||
@ -871,8 +897,8 @@ Serial.println(tempaccel.z, DEC);
|
||||
//---------------------------------
|
||||
case 4:
|
||||
medium_loopCounter = 0;
|
||||
delta_ms_medium_loop = millis() - medium_loopTimer;
|
||||
medium_loopTimer = millis();
|
||||
delta_ms_medium_loop = millis() - medium_loopTimer_ms;
|
||||
medium_loopTimer_ms = millis();
|
||||
|
||||
if (g.battery_monitoring != 0){
|
||||
read_battery();
|
||||
@ -1001,16 +1027,16 @@ static void update_current_flight_mode(void)
|
||||
if (hold_course != -1) {
|
||||
calc_nav_roll();
|
||||
} else {
|
||||
nav_roll = 0;
|
||||
nav_roll_cd = 0;
|
||||
}
|
||||
|
||||
if(airspeed.use()) {
|
||||
calc_nav_pitch();
|
||||
if(nav_pitch < (long)takeoff_pitch)
|
||||
nav_pitch = (long)takeoff_pitch;
|
||||
if (nav_pitch_cd < takeoff_pitch_cd)
|
||||
nav_pitch_cd = takeoff_pitch_cd;
|
||||
} else {
|
||||
nav_pitch = (long)((float)g_gps->ground_speed / (float)g.airspeed_cruise_cm * (float)takeoff_pitch * 0.5);
|
||||
nav_pitch = constrain(nav_pitch, 500l, (long)takeoff_pitch);
|
||||
nav_pitch_cd = (float)g_gps->ground_speed / (float)g.airspeed_cruise_cm * (float)takeoff_pitch_cd * 0.5;
|
||||
nav_pitch_cd = constrain(nav_pitch_cd, 500, takeoff_pitch_cd);
|
||||
}
|
||||
|
||||
g.channel_throttle.servo_out = g.throttle_max; //TODO: Replace with THROTTLE_TAKEOFF or other method of controlling throttle
|
||||
@ -1028,7 +1054,7 @@ static void update_current_flight_mode(void)
|
||||
}else{
|
||||
calc_nav_pitch(); // calculate nav_pitch just to use for calc_throttle
|
||||
calc_throttle(); // throttle based on altitude error
|
||||
nav_pitch = g.land_pitch_cd; // pitch held constant
|
||||
nav_pitch_cd = g.land_pitch_cd; // pitch held constant
|
||||
}
|
||||
|
||||
if (land_complete) {
|
||||
@ -1063,11 +1089,13 @@ static void update_current_flight_mode(void)
|
||||
|
||||
case FLY_BY_WIRE_A:
|
||||
// set nav_roll and nav_pitch using sticks
|
||||
nav_roll = g.channel_roll.norm_input() * g.roll_limit;
|
||||
nav_pitch = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min;
|
||||
nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd;
|
||||
nav_pitch_cd = g.channel_pitch.norm_input() * (-1) * g.pitch_limit_min_cd;
|
||||
// We use pitch_min above because it is usually greater magnitude then pitch_max. -1 is to compensate for its sign.
|
||||
nav_pitch = constrain(nav_pitch, -3000, 3000); // trying to give more pitch authority
|
||||
if (inverted_flight) nav_pitch = -nav_pitch;
|
||||
nav_pitch_cd = constrain(nav_pitch_cd, -3000, 3000); // trying to give more pitch authority
|
||||
if (inverted_flight) {
|
||||
nav_pitch_cd = -nav_pitch_cd;
|
||||
}
|
||||
break;
|
||||
|
||||
case FLY_BY_WIRE_B:
|
||||
@ -1077,23 +1105,25 @@ static void update_current_flight_mode(void)
|
||||
|
||||
// Thanks to Yury MonZon for the altitude limit code!
|
||||
|
||||
nav_roll = g.channel_roll.norm_input() * g.roll_limit;
|
||||
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
|
||||
nav_roll_cd = g.channel_roll.norm_input() * g.roll_limit_cd;
|
||||
altitude_error_cm = g.channel_pitch.norm_input() * g.pitch_limit_min_cd;
|
||||
|
||||
if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == 0)) {
|
||||
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
|
||||
if ((current_loc.alt >= home.alt+g.FBWB_min_altitude_cm) || (g.FBWB_min_altitude_cm == 0)) {
|
||||
altitude_error_cm = g.channel_pitch.norm_input() * g.pitch_limit_min_cd;
|
||||
} else {
|
||||
if (g.channel_pitch.norm_input()<0)
|
||||
altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min ;
|
||||
else altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) ;
|
||||
if (g.channel_pitch.norm_input()<0) {
|
||||
altitude_error_cm =( (home.alt + g.FBWB_min_altitude_cm) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min_cd;
|
||||
} else {
|
||||
altitude_error_cm =( (home.alt + g.FBWB_min_altitude_cm) - current_loc.alt);
|
||||
}
|
||||
}
|
||||
calc_throttle();
|
||||
calc_nav_pitch();
|
||||
break;
|
||||
|
||||
case STABILIZE:
|
||||
nav_roll = 0;
|
||||
nav_pitch = 0;
|
||||
nav_roll_cd = 0;
|
||||
nav_pitch_cd = 0;
|
||||
// throttle is passthrough
|
||||
break;
|
||||
|
||||
@ -1101,8 +1131,8 @@ static void update_current_flight_mode(void)
|
||||
// we have no GPS installed and have lost radio contact
|
||||
// or we just want to fly around in a gentle circle w/o GPS
|
||||
// ----------------------------------------------------
|
||||
nav_roll = g.roll_limit / 3;
|
||||
nav_pitch = 0;
|
||||
nav_roll_cd = g.roll_limit_cd / 3;
|
||||
nav_pitch_cd = 0;
|
||||
|
||||
if (failsafe != FAILSAFE_NONE){
|
||||
g.channel_throttle.servo_out = g.throttle_cruise;
|
||||
|
||||
@ -30,7 +30,7 @@ static void stabilize()
|
||||
}
|
||||
|
||||
if(crash_timer > 0){
|
||||
nav_roll = 0;
|
||||
nav_roll_cd = 0;
|
||||
}
|
||||
|
||||
if (inverted_flight) {
|
||||
@ -39,8 +39,8 @@ static void stabilize()
|
||||
// would really confuse the PID code. The easiest way to
|
||||
// handle this is to ensure both go in the same direction from
|
||||
// zero
|
||||
nav_roll += 18000;
|
||||
if (ahrs.roll_sensor < 0) nav_roll -= 36000;
|
||||
nav_roll_cd += 18000;
|
||||
if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
|
||||
}
|
||||
|
||||
// For Testing Only
|
||||
@ -50,11 +50,11 @@ static void stabilize()
|
||||
|
||||
// Calculate dersired servo output for the roll
|
||||
// ---------------------------------------------
|
||||
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - ahrs.roll_sensor), speed_scaler);
|
||||
long tempcalc = nav_pitch +
|
||||
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll_cd - ahrs.roll_sensor), speed_scaler);
|
||||
int32_t tempcalc = nav_pitch_cd +
|
||||
fabs(ahrs.roll_sensor * g.kff_pitch_compensation) +
|
||||
(g.channel_throttle.servo_out * g.kff_throttle_to_pitch) -
|
||||
(ahrs.pitch_sensor - g.pitch_trim);
|
||||
(ahrs.pitch_sensor - g.pitch_trim_cd);
|
||||
if (inverted_flight) {
|
||||
// when flying upside down the elevator control is inverted
|
||||
tempcalc = -tempcalc;
|
||||
@ -142,17 +142,17 @@ static void calc_throttle()
|
||||
// no airspeed sensor, we use nav pitch to determine the proper throttle output
|
||||
// AUTO, RTL, etc
|
||||
// ---------------------------------------------------------------------------
|
||||
if (nav_pitch >= 0) {
|
||||
g.channel_throttle.servo_out = throttle_target + (g.throttle_max - throttle_target) * nav_pitch / g.pitch_limit_max;
|
||||
if (nav_pitch_cd >= 0) {
|
||||
g.channel_throttle.servo_out = throttle_target + (g.throttle_max - throttle_target) * nav_pitch_cd / g.pitch_limit_max_cd;
|
||||
} else {
|
||||
g.channel_throttle.servo_out = throttle_target - (throttle_target - g.throttle_min) * nav_pitch / g.pitch_limit_min;
|
||||
g.channel_throttle.servo_out = throttle_target - (throttle_target - g.throttle_min) * nav_pitch_cd / g.pitch_limit_min_cd;
|
||||
}
|
||||
|
||||
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
|
||||
} else {
|
||||
// throttle control with airspeed compensation
|
||||
// -------------------------------------------
|
||||
energy_error = airspeed_energy_error + (float)altitude_error * 0.098f;
|
||||
energy_error = airspeed_energy_error + altitude_error_cm * 0.098f;
|
||||
|
||||
// positive energy errors make the throttle go higher
|
||||
g.channel_throttle.servo_out = g.throttle_cruise + g.pidTeThrottle.get_pid(energy_error);
|
||||
@ -190,11 +190,11 @@ static void calc_nav_pitch()
|
||||
// Calculate the Pitch of the plane
|
||||
// --------------------------------
|
||||
if (airspeed.use()) {
|
||||
nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error);
|
||||
nav_pitch_cd = -g.pidNavPitchAirspeed.get_pid(airspeed_error_cm);
|
||||
} else {
|
||||
nav_pitch = g.pidNavPitchAltitude.get_pid(altitude_error);
|
||||
nav_pitch_cd = g.pidNavPitchAltitude.get_pid(altitude_error_cm);
|
||||
}
|
||||
nav_pitch = constrain(nav_pitch, g.pitch_limit_min.get(), g.pitch_limit_max.get());
|
||||
nav_pitch_cd = constrain(nav_pitch_cd, g.pitch_limit_min_cd.get(), g.pitch_limit_max_cd.get());
|
||||
}
|
||||
|
||||
|
||||
@ -205,7 +205,7 @@ static void calc_nav_roll()
|
||||
// Scale from centidegrees (PID input) to radians per second. A P gain of 1.0 should result in a
|
||||
// desired rate of 1 degree per second per degree of error - if you're 15 degrees off, you'll try
|
||||
// to turn at 15 degrees per second.
|
||||
float turn_rate = ToRad(g.pidNavRoll.get_pid(bearing_error) * .01);
|
||||
float turn_rate = ToRad(g.pidNavRoll.get_pid(bearing_error_cd) * .01);
|
||||
|
||||
// Use airspeed_cruise as an analogue for airspeed if we don't have airspeed.
|
||||
float speed;
|
||||
@ -228,10 +228,10 @@ static void calc_nav_roll()
|
||||
// then remove for a future release
|
||||
float nav_gain_scaler = 0.01 * g_gps->ground_speed / g.scaling_speed;
|
||||
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
|
||||
nav_roll = g.pidNavRoll.get_pid(bearing_error, nav_gain_scaler); //returns desired bank angle in degrees*100
|
||||
nav_roll_cd = g.pidNavRoll.get_pid(bearing_error_cd, nav_gain_scaler); //returns desired bank angle in degrees*100
|
||||
#endif
|
||||
|
||||
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
|
||||
nav_roll_cd = constrain(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get());
|
||||
}
|
||||
|
||||
|
||||
@ -392,9 +392,9 @@ static void set_servos(void)
|
||||
} else if (control_mode >= FLY_BY_WIRE_B) {
|
||||
// FIXME: use target_airspeed in both FBW_B and g.airspeed_enabled cases - Doug?
|
||||
if (control_mode == FLY_BY_WIRE_B) {
|
||||
flapSpeedSource = ((float)target_airspeed)/100;
|
||||
flapSpeedSource = target_airspeed_cm * 0.01;
|
||||
} else if (airspeed.use()) {
|
||||
flapSpeedSource = g.airspeed_cruise_cm/100;
|
||||
flapSpeedSource = g.airspeed_cruise_cm * 0.01;
|
||||
} else {
|
||||
flapSpeedSource = g.throttle_cruise;
|
||||
}
|
||||
|
||||
@ -106,7 +106,7 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
|
||||
chan,
|
||||
millis(),
|
||||
ahrs.roll,
|
||||
ahrs.pitch - radians(g.pitch_trim*0.01),
|
||||
ahrs.pitch - radians(g.pitch_trim_cd*0.01),
|
||||
ahrs.yaw,
|
||||
omega.x,
|
||||
omega.y,
|
||||
@ -251,16 +251,16 @@ static void NOINLINE send_location(mavlink_channel_t chan)
|
||||
|
||||
static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
||||
{
|
||||
int16_t bearing = (hold_course==-1?nav_bearing:hold_course) / 100;
|
||||
int16_t bearing = (hold_course==-1?nav_bearing_cd:hold_course) / 100;
|
||||
mavlink_msg_nav_controller_output_send(
|
||||
chan,
|
||||
nav_roll / 1.0e2,
|
||||
nav_pitch / 1.0e2,
|
||||
nav_roll_cd * 0.01,
|
||||
nav_pitch_cd * 0.01,
|
||||
bearing,
|
||||
target_bearing / 100,
|
||||
target_bearing_cd * 0.01,
|
||||
wp_distance,
|
||||
altitude_error / 1.0e2,
|
||||
airspeed_error,
|
||||
altitude_error_cm * 0.01,
|
||||
airspeed_error_cm,
|
||||
crosstrack_error);
|
||||
}
|
||||
|
||||
@ -1664,7 +1664,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
mavlink_msg_hil_state_decode(msg, &packet);
|
||||
|
||||
float vel = sqrt((packet.vx * (float)packet.vx) + (packet.vy * (float)packet.vy));
|
||||
float cog = wrap_360(ToDeg(atan2(packet.vx, packet.vy)) * 100);
|
||||
float cog = wrap_360_cd(ToDeg(atan2(packet.vx, packet.vy)) * 100);
|
||||
|
||||
// set gps hil sensor
|
||||
g_gps->setHIL(packet.time_usec/1000,
|
||||
|
||||
@ -290,11 +290,11 @@ static void Log_Write_Control_Tuning()
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
|
||||
DataFlash.WriteInt((int)(g.channel_roll.servo_out));
|
||||
DataFlash.WriteInt((int)nav_roll);
|
||||
DataFlash.WriteInt(g.channel_roll.servo_out);
|
||||
DataFlash.WriteInt(nav_roll_cd);
|
||||
DataFlash.WriteInt((int)ahrs.roll_sensor);
|
||||
DataFlash.WriteInt((int)(g.channel_pitch.servo_out));
|
||||
DataFlash.WriteInt((int)nav_pitch);
|
||||
DataFlash.WriteInt((int)nav_pitch_cd);
|
||||
DataFlash.WriteInt((int)ahrs.pitch_sensor);
|
||||
DataFlash.WriteInt((int)(g.channel_throttle.servo_out));
|
||||
DataFlash.WriteInt((int)(g.channel_rudder.servo_out));
|
||||
@ -311,9 +311,9 @@ static void Log_Write_Nav_Tuning()
|
||||
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
|
||||
DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor);
|
||||
DataFlash.WriteInt((int16_t)wp_distance);
|
||||
DataFlash.WriteInt((uint16_t)target_bearing);
|
||||
DataFlash.WriteInt((uint16_t)nav_bearing);
|
||||
DataFlash.WriteInt(altitude_error);
|
||||
DataFlash.WriteInt(target_bearing_cd);
|
||||
DataFlash.WriteInt(nav_bearing_cd);
|
||||
DataFlash.WriteInt(altitude_error_cm);
|
||||
DataFlash.WriteInt((int16_t)airspeed.get_airspeed_cm());
|
||||
DataFlash.WriteInt(0); // was nav_gain_scaler
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
|
||||
@ -50,13 +50,13 @@ public:
|
||||
//
|
||||
k_param_format_version = 0,
|
||||
k_param_software_type,
|
||||
k_param_num_resets,
|
||||
|
||||
// Misc
|
||||
//
|
||||
k_param_auto_trim,
|
||||
k_param_switch_enable, // UNUSED
|
||||
k_param_auto_trim = 10,
|
||||
k_param_log_bitmask,
|
||||
k_param_pitch_trim,
|
||||
k_param_pitch_trim_cd,
|
||||
k_param_mix_mode,
|
||||
k_param_reverse_elevons,
|
||||
k_param_reverse_ch1_elevon,
|
||||
@ -65,8 +65,6 @@ public:
|
||||
k_param_flap_1_speed,
|
||||
k_param_flap_2_percent,
|
||||
k_param_flap_2_speed,
|
||||
k_param_num_resets,
|
||||
k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change
|
||||
k_param_reset_switch_chan,
|
||||
k_param_manual_level,
|
||||
k_param_land_pitch_cd,
|
||||
@ -84,29 +82,23 @@ public:
|
||||
//
|
||||
k_param_flybywire_airspeed_min = 120,
|
||||
k_param_flybywire_airspeed_max,
|
||||
k_param_FBWB_min_altitude, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
|
||||
k_param_FBWB_min_altitude_cm, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
|
||||
|
||||
//
|
||||
// 130: Sensor parameters
|
||||
//
|
||||
k_param_imu = 130, // sensor calibration
|
||||
k_param_altitude_mix,
|
||||
k_param_airspeed_ratio, // UNUSED
|
||||
k_param_ground_pressure, // UNUSED
|
||||
k_param_ground_temperature, // UNUSED
|
||||
|
||||
k_param_compass_enabled = 135,
|
||||
k_param_compass_enabled,
|
||||
k_param_compass,
|
||||
k_param_battery_monitoring,
|
||||
k_param_volt_div_ratio,
|
||||
k_param_curr_amp_per_volt,
|
||||
k_param_input_voltage,
|
||||
k_param_pack_capacity,
|
||||
k_param_airspeed_offset, // UNUSED
|
||||
k_param_sonar_enabled,
|
||||
k_param_airspeed_enabled, // UNUSED
|
||||
k_param_ahrs, // AHRS group
|
||||
k_param_airspeed_use, // UNUSED
|
||||
k_param_barometer, // barometer ground calibration
|
||||
k_param_airspeed, // AP_Airspeed parameters
|
||||
|
||||
@ -115,19 +107,19 @@ public:
|
||||
//
|
||||
k_param_crosstrack_gain = 150,
|
||||
k_param_crosstrack_entry_angle,
|
||||
k_param_roll_limit,
|
||||
k_param_pitch_limit_max,
|
||||
k_param_pitch_limit_min,
|
||||
k_param_roll_limit_cd,
|
||||
k_param_pitch_limit_max_cd,
|
||||
k_param_pitch_limit_min_cd,
|
||||
k_param_airspeed_cruise_cm,
|
||||
k_param_RTL_altitude,
|
||||
k_param_RTL_altitude_cm,
|
||||
k_param_inverted_flight_ch,
|
||||
k_param_min_gndspeed,
|
||||
k_param_min_gndspeed_cm,
|
||||
|
||||
|
||||
//
|
||||
// Camera and mount parameters
|
||||
//
|
||||
k_param_camera = 159,
|
||||
k_param_camera = 160,
|
||||
k_param_camera_mount,
|
||||
|
||||
//
|
||||
@ -141,6 +133,9 @@ public:
|
||||
k_param_rc_6,
|
||||
k_param_rc_7,
|
||||
k_param_rc_8,
|
||||
k_param_rc_9,
|
||||
k_param_rc_10,
|
||||
k_param_rc_11,
|
||||
|
||||
k_param_throttle_min,
|
||||
k_param_throttle_max,
|
||||
@ -153,9 +148,6 @@ public:
|
||||
k_param_gcs_heartbeat_fs_enabled,
|
||||
k_param_throttle_slewrate,
|
||||
|
||||
k_param_rc_9,
|
||||
k_param_rc_10,
|
||||
k_param_rc_11,
|
||||
//
|
||||
// 200: Feed-forward gains
|
||||
//
|
||||
@ -195,56 +187,12 @@ public:
|
||||
|
||||
//
|
||||
// 240: PID Controllers
|
||||
//
|
||||
// Heading-to-roll PID:
|
||||
// heading error from command to roll command deviation from trim
|
||||
// (bank to turn strategy)
|
||||
//
|
||||
k_param_pidNavRoll = 240,
|
||||
|
||||
// Roll-to-servo PID:
|
||||
// roll error from command to roll servo deviation from trim
|
||||
// (tracks commanded bank angle)
|
||||
//
|
||||
k_param_pidServoRoll,
|
||||
|
||||
//
|
||||
// Pitch control
|
||||
//
|
||||
// Pitch-to-servo PID:
|
||||
// pitch error from command to pitch servo deviation from trim
|
||||
// (front-side strategy)
|
||||
//
|
||||
k_param_pidServoPitch,
|
||||
|
||||
// Airspeed-to-pitch PID:
|
||||
// airspeed error from command to pitch servo deviation from trim
|
||||
// (back-side strategy)
|
||||
//
|
||||
k_param_pidNavPitchAirspeed,
|
||||
|
||||
//
|
||||
// Yaw control
|
||||
//
|
||||
// Yaw-to-servo PID:
|
||||
// yaw rate error from command to yaw servo deviation from trim
|
||||
// (stabilizes dutch roll)
|
||||
//
|
||||
k_param_pidServoRudder,
|
||||
|
||||
//
|
||||
// Throttle control
|
||||
//
|
||||
// Energy-to-throttle PID:
|
||||
// total energy error from command to throttle servo deviation from trim
|
||||
// (throttle back-side strategy alternative)
|
||||
//
|
||||
k_param_pidTeThrottle,
|
||||
|
||||
// Altitude-to-pitch PID:
|
||||
// altitude error from command to pitch servo deviation from trim
|
||||
// (throttle front-side strategy alternative)
|
||||
//
|
||||
k_param_pidNavPitchAltitude,
|
||||
|
||||
// 254,255: reserved
|
||||
@ -283,8 +231,9 @@ public:
|
||||
AP_Int8 waypoint_mode;
|
||||
AP_Int8 command_total;
|
||||
AP_Int8 command_index;
|
||||
AP_Int8 waypoint_radius;
|
||||
AP_Int8 loiter_radius;
|
||||
AP_Int16 waypoint_radius;
|
||||
AP_Int16 loiter_radius;
|
||||
|
||||
#if GEOFENCE_ENABLED == ENABLED
|
||||
AP_Int8 fence_action;
|
||||
AP_Int8 fence_total;
|
||||
@ -295,8 +244,8 @@ public:
|
||||
|
||||
// Fly-by-wire
|
||||
//
|
||||
AP_Int8 flybywire_airspeed_min;
|
||||
AP_Int8 flybywire_airspeed_max;
|
||||
AP_Int16 flybywire_airspeed_min;
|
||||
AP_Int16 flybywire_airspeed_max;
|
||||
|
||||
// Throttle
|
||||
//
|
||||
@ -324,9 +273,9 @@ public:
|
||||
|
||||
// Navigational maneuvering limits
|
||||
//
|
||||
AP_Int16 roll_limit;
|
||||
AP_Int16 pitch_limit_max;
|
||||
AP_Int16 pitch_limit_min;
|
||||
AP_Int16 roll_limit_cd;
|
||||
AP_Int16 pitch_limit_max_cd;
|
||||
AP_Int16 pitch_limit_min_cd;
|
||||
|
||||
// Misc
|
||||
//
|
||||
@ -337,18 +286,16 @@ public:
|
||||
AP_Int8 reverse_ch2_elevon;
|
||||
AP_Int16 num_resets;
|
||||
AP_Int16 log_bitmask;
|
||||
AP_Int16 log_last_filenumber; // *** Deprecated - remove with next eeprom number change
|
||||
AP_Int8 reset_switch_chan;
|
||||
AP_Int8 manual_level;
|
||||
AP_Int16 airspeed_cruise_cm;
|
||||
AP_Int16 RTL_altitude;
|
||||
AP_Int32 airspeed_cruise_cm;
|
||||
AP_Int32 RTL_altitude_cm;
|
||||
AP_Int16 land_pitch_cd;
|
||||
AP_Int16 min_gndspeed;
|
||||
AP_Int16 pitch_trim;
|
||||
AP_Int16 FBWB_min_altitude;
|
||||
AP_Int32 min_gndspeed_cm;
|
||||
AP_Int16 pitch_trim_cd;
|
||||
AP_Int16 FBWB_min_altitude_cm;
|
||||
|
||||
AP_Int8 compass_enabled;
|
||||
AP_Int16 angle_of_attack;
|
||||
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current
|
||||
AP_Int8 flap_1_percent;
|
||||
AP_Int8 flap_1_speed;
|
||||
@ -357,7 +304,7 @@ public:
|
||||
AP_Float volt_div_ratio;
|
||||
AP_Float curr_amp_per_volt;
|
||||
AP_Float input_voltage;
|
||||
AP_Int16 pack_capacity; // Battery pack capacity less reserve
|
||||
AP_Int32 pack_capacity; // Battery pack capacity less reserve
|
||||
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
|
||||
AP_Int8 sonar_enabled;
|
||||
|
||||
|
||||
@ -302,7 +302,7 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Range: 0 9000
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(roll_limit, "LIM_ROLL_CD", HEAD_MAX_CENTIDEGREE),
|
||||
GSCALAR(roll_limit_cd, "LIM_ROLL_CD", HEAD_MAX_CENTIDEGREE),
|
||||
|
||||
// @Param: LIM_PITCH_MAX
|
||||
// @DisplayName: Maximum Pitch Angle
|
||||
@ -311,7 +311,7 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Range: 0 9000
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(pitch_limit_max, "LIM_PITCH_MAX", PITCH_MAX_CENTIDEGREE),
|
||||
GSCALAR(pitch_limit_max_cd, "LIM_PITCH_MAX", PITCH_MAX_CENTIDEGREE),
|
||||
|
||||
// @Param: LIM_PITCH_MIN
|
||||
// @DisplayName: Minimum Pitch Angle
|
||||
@ -320,7 +320,7 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Range: 0 9000
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(pitch_limit_min, "LIM_PITCH_MIN", PITCH_MIN_CENTIDEGREE),
|
||||
GSCALAR(pitch_limit_min_cd, "LIM_PITCH_MIN", PITCH_MIN_CENTIDEGREE),
|
||||
|
||||
// @Param: AUTO_TRIM
|
||||
// @DisplayName: Auto trim
|
||||
@ -370,8 +370,6 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @User: Advanced
|
||||
GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK),
|
||||
|
||||
GSCALAR(log_last_filenumber, "LOG_LASTFILE", 0),
|
||||
|
||||
// @Param: RST_SWITCH_CH
|
||||
// @DisplayName: Reset Switch Channel
|
||||
// @Description: RC channel to use to reset to last flight mode after geofence takeover
|
||||
@ -397,23 +395,23 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Description: Minimum ground speed in cm/s when under airspeed control
|
||||
// @Units: cm/s
|
||||
// @User: Advanced
|
||||
GSCALAR(min_gndspeed, "MIN_GNDSPD_CM", MIN_GNDSPEED_CM),
|
||||
GSCALAR(min_gndspeed_cm, "MIN_GNDSPD_CM", MIN_GNDSPEED_CM),
|
||||
|
||||
// @Param: TRIM_PITCH_CD
|
||||
// @DisplayName: Pitch angle offset
|
||||
// @Description: offset to add to pitch - used for trimming tail draggers
|
||||
// @Units: centi-Degrees
|
||||
// @User: Advanced
|
||||
GSCALAR(pitch_trim, "TRIM_PITCH_CD", 0),
|
||||
GSCALAR(pitch_trim_cd, "TRIM_PITCH_CD", 0),
|
||||
|
||||
// @Param: ALT_HOLD_RTL
|
||||
// @DisplayName: RTL altitude
|
||||
// @Description: Return to launch target altitude
|
||||
// @Units: centimeters
|
||||
// @User: User
|
||||
GSCALAR(RTL_altitude, "ALT_HOLD_RTL", ALT_HOLD_HOME_CM),
|
||||
GSCALAR(RTL_altitude_cm, "ALT_HOLD_RTL", ALT_HOLD_HOME_CM),
|
||||
|
||||
GSCALAR(FBWB_min_altitude, "ALT_HOLD_FBWCM", ALT_HOLD_FBW_CM),
|
||||
GSCALAR(FBWB_min_altitude_cm, "ALT_HOLD_FBWCM", ALT_HOLD_FBW_CM),
|
||||
|
||||
// @Param: MAG_ENABLE
|
||||
// @DisplayName: Enable Compass
|
||||
|
||||
@ -122,10 +122,10 @@ static void decrement_cmd_index()
|
||||
|
||||
static long read_alt_to_hold()
|
||||
{
|
||||
if(g.RTL_altitude < 0)
|
||||
if (g.RTL_altitude_cm < 0) {
|
||||
return current_loc.alt;
|
||||
else
|
||||
return g.RTL_altitude + home.alt;
|
||||
}
|
||||
return g.RTL_altitude_cm + home.alt;
|
||||
}
|
||||
|
||||
|
||||
@ -155,12 +155,12 @@ static void set_next_WP(struct Location *wp)
|
||||
|
||||
// used to control FBW and limit the rate of climb
|
||||
// -----------------------------------------------
|
||||
target_altitude = current_loc.alt;
|
||||
target_altitude_cm = current_loc.alt;
|
||||
|
||||
if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND))
|
||||
offset_altitude = next_WP.alt - prev_WP.alt;
|
||||
offset_altitude_cm = next_WP.alt - prev_WP.alt;
|
||||
else
|
||||
offset_altitude = 0;
|
||||
offset_altitude_cm = 0;
|
||||
|
||||
// zero out our loiter vals to watch for missed waypoints
|
||||
loiter_delta = 0;
|
||||
@ -170,12 +170,12 @@ static void set_next_WP(struct Location *wp)
|
||||
// this is handy for the groundstation
|
||||
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing = get_bearing(¤t_loc, &next_WP);
|
||||
nav_bearing = target_bearing;
|
||||
target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP);
|
||||
nav_bearing_cd = target_bearing_cd;
|
||||
|
||||
// to check if we have missed the WP
|
||||
// ----------------------------
|
||||
old_target_bearing = target_bearing;
|
||||
old_target_bearing_cd = target_bearing_cd;
|
||||
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
@ -194,17 +194,17 @@ static void set_guided_WP(void)
|
||||
|
||||
// used to control FBW and limit the rate of climb
|
||||
// -----------------------------------------------
|
||||
target_altitude = current_loc.alt;
|
||||
offset_altitude = next_WP.alt - prev_WP.alt;
|
||||
target_altitude_cm = current_loc.alt;
|
||||
offset_altitude_cm = next_WP.alt - prev_WP.alt;
|
||||
|
||||
// this is handy for the groundstation
|
||||
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing = get_bearing(¤t_loc, &next_WP);
|
||||
target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP);
|
||||
|
||||
// to check if we have missed the WP
|
||||
// ----------------------------
|
||||
old_target_bearing = target_bearing;
|
||||
old_target_bearing_cd = target_bearing_cd;
|
||||
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
@ -242,7 +242,7 @@ void init_home()
|
||||
// Load home for a default guided_WP
|
||||
// -------------
|
||||
guided_WP = home;
|
||||
guided_WP.alt += g.RTL_altitude;
|
||||
guided_WP.alt += g.RTL_altitude_cm;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -247,7 +247,7 @@ static void do_takeoff()
|
||||
{
|
||||
set_next_WP(&next_nav_command);
|
||||
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
|
||||
takeoff_pitch = (int)next_nav_command.p1 * 100;
|
||||
takeoff_pitch_cd = (int)next_nav_command.p1 * 100;
|
||||
//Serial.printf_P(PSTR("TO pitch:")); Serial.println(takeoff_pitch);
|
||||
//Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt);
|
||||
takeoff_altitude = next_nav_command.alt;
|
||||
@ -282,8 +282,8 @@ static void do_loiter_turns()
|
||||
static void do_loiter_time()
|
||||
{
|
||||
set_next_WP(&next_nav_command);
|
||||
loiter_time = millis();
|
||||
loiter_time_max = next_nav_command.p1; // units are (seconds * 10)
|
||||
loiter_time_ms = millis();
|
||||
loiter_time_max_ms = next_nav_command.p1 * (uint32_t)100; // units are (seconds * 10)
|
||||
}
|
||||
|
||||
/********************************************************************************/
|
||||
@ -304,7 +304,7 @@ static bool verify_takeoff()
|
||||
|
||||
if (hold_course != -1) {
|
||||
// recalc bearing error with hold_course;
|
||||
nav_bearing = hold_course;
|
||||
nav_bearing_cd = hold_course;
|
||||
// recalc bearing error
|
||||
calc_bearing_error();
|
||||
}
|
||||
@ -348,7 +348,7 @@ static bool verify_land()
|
||||
|
||||
if (hold_course != -1){
|
||||
// recalc bearing error with hold_course;
|
||||
nav_bearing = hold_course;
|
||||
nav_bearing_cd = hold_course;
|
||||
// recalc bearing error
|
||||
calc_bearing_error();
|
||||
}
|
||||
@ -396,7 +396,7 @@ static bool verify_loiter_time()
|
||||
{
|
||||
update_loiter();
|
||||
calc_bearing_error();
|
||||
if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds
|
||||
if ((millis() - loiter_time_ms) > loiter_time_max_ms) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
|
||||
return true;
|
||||
}
|
||||
@ -441,9 +441,9 @@ static void do_change_alt()
|
||||
condition_rate = abs((int)next_nonnav_command.lat);
|
||||
condition_value = next_nonnav_command.alt;
|
||||
if(condition_value < current_loc.alt) condition_rate = -condition_rate;
|
||||
target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
|
||||
target_altitude_cm = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update
|
||||
next_WP.alt = condition_value; // For future nav calculations
|
||||
offset_altitude = 0; // For future nav calculations
|
||||
offset_altitude_cm = 0; // For future nav calculations
|
||||
}
|
||||
|
||||
static void do_within_distance()
|
||||
@ -470,7 +470,7 @@ static bool verify_change_alt()
|
||||
condition_value = 0;
|
||||
return true;
|
||||
}
|
||||
target_altitude += condition_rate / 10;
|
||||
target_altitude_cm += condition_rate / 10;
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -531,7 +531,7 @@ static void do_change_speed()
|
||||
g.airspeed_cruise_cm.set(next_nonnav_command.alt * 100);
|
||||
break;
|
||||
case 1: // Ground speed
|
||||
g.min_gndspeed.set(next_nonnav_command.alt * 100);
|
||||
g.min_gndspeed_cm.set(next_nonnav_command.alt * 100);
|
||||
break;
|
||||
}
|
||||
|
||||
@ -574,8 +574,8 @@ static void do_repeat_servo()
|
||||
|
||||
if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) {
|
||||
|
||||
event_timer = 0;
|
||||
event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
|
||||
event_timer_ms = 0;
|
||||
event_delay_ms = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
|
||||
event_repeat = next_nonnav_command.lat * 2;
|
||||
event_value = next_nonnav_command.alt;
|
||||
|
||||
@ -600,8 +600,8 @@ static void do_repeat_servo()
|
||||
static void do_repeat_relay()
|
||||
{
|
||||
event_id = RELAY_TOGGLE;
|
||||
event_timer = 0;
|
||||
event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
|
||||
event_timer_ms = 0;
|
||||
event_delay_ms = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds)
|
||||
event_repeat = next_nonnav_command.alt * 2;
|
||||
update_events();
|
||||
}
|
||||
|
||||
@ -92,7 +92,7 @@ void low_battery_event(void)
|
||||
|
||||
static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
|
||||
{
|
||||
if(event_repeat == 0 || (millis() - event_timer) < event_delay)
|
||||
if(event_repeat == 0 || (millis() - event_timer_ms) < event_delay_ms)
|
||||
return;
|
||||
|
||||
if (event_repeat > 0){
|
||||
@ -100,7 +100,7 @@ static void update_events(void) // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_
|
||||
}
|
||||
|
||||
if(event_repeat != 0) { // event_repeat = -1 means repeat forever
|
||||
event_timer = millis();
|
||||
event_timer_ms = millis();
|
||||
|
||||
if (event_id >= CH_5 && event_id <= CH_8) {
|
||||
if(event_repeat%2) {
|
||||
|
||||
@ -261,7 +261,7 @@ static void geofence_check(bool altitude_check_only)
|
||||
// min and max
|
||||
if (g.fence_minalt >= g.fence_maxalt) {
|
||||
// invalid min/max, use RTL_altitude
|
||||
guided_WP.alt = home.alt + g.RTL_altitude;
|
||||
guided_WP.alt = home.alt + g.RTL_altitude_cm;
|
||||
} else {
|
||||
guided_WP.alt = home.alt + 100.0*(g.fence_minalt + g.fence_maxalt)/2;
|
||||
}
|
||||
|
||||
@ -29,17 +29,17 @@ static void navigate()
|
||||
|
||||
// target_bearing is where we should be heading
|
||||
// --------------------------------------------
|
||||
target_bearing = get_bearing(¤t_loc, &next_WP);
|
||||
target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP);
|
||||
|
||||
// nav_bearing will includes xtrac correction
|
||||
// ------------------------------------------
|
||||
nav_bearing = target_bearing;
|
||||
nav_bearing_cd = target_bearing_cd;
|
||||
|
||||
// check if we have missed the WP
|
||||
loiter_delta = (target_bearing - old_target_bearing)/100;
|
||||
loiter_delta = (target_bearing_cd - old_target_bearing_cd)/100;
|
||||
|
||||
// reset the old value
|
||||
old_target_bearing = target_bearing;
|
||||
old_target_bearing_cd = target_bearing_cd;
|
||||
|
||||
// wrap values
|
||||
if (loiter_delta > 180) loiter_delta -= 360;
|
||||
@ -56,7 +56,7 @@ static void navigate()
|
||||
// Disabled for now
|
||||
void calc_distance_error()
|
||||
{
|
||||
distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error * .01));
|
||||
distance_estimate += (float)g_gps->ground_speed * .0002 * cos(radians(bearing_error_cd * .01));
|
||||
distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_wp_distance);
|
||||
wp_distance = max(distance_estimate,10);
|
||||
}
|
||||
@ -67,11 +67,11 @@ static void calc_airspeed_errors()
|
||||
float aspeed_cm = airspeed.get_airspeed_cm();
|
||||
|
||||
// Normal airspeed target
|
||||
target_airspeed = g.airspeed_cruise_cm;
|
||||
target_airspeed_cm = g.airspeed_cruise_cm;
|
||||
|
||||
// FBW_B airspeed target
|
||||
if (control_mode == FLY_BY_WIRE_B) {
|
||||
target_airspeed = ((int)(g.flybywire_airspeed_max -
|
||||
target_airspeed_cm = ((int)(g.flybywire_airspeed_max -
|
||||
g.flybywire_airspeed_min) *
|
||||
g.channel_throttle.servo_out) +
|
||||
((int)g.flybywire_airspeed_min * 100);
|
||||
@ -80,29 +80,29 @@ static void calc_airspeed_errors()
|
||||
// Set target to current airspeed + ground speed undershoot,
|
||||
// but only when this is faster than the target airspeed commanded
|
||||
// above.
|
||||
if (control_mode >= FLY_BY_WIRE_B && (g.min_gndspeed > 0)) {
|
||||
long min_gnd_target_airspeed = aspeed_cm + groundspeed_undershoot;
|
||||
if (min_gnd_target_airspeed > target_airspeed)
|
||||
target_airspeed = min_gnd_target_airspeed;
|
||||
if (control_mode >= FLY_BY_WIRE_B && (g.min_gndspeed_cm > 0)) {
|
||||
int32_t min_gnd_target_airspeed = aspeed_cm + groundspeed_undershoot;
|
||||
if (min_gnd_target_airspeed > target_airspeed_cm)
|
||||
target_airspeed_cm = min_gnd_target_airspeed;
|
||||
}
|
||||
|
||||
// Bump up the target airspeed based on throttle nudging
|
||||
if (control_mode >= AUTO && airspeed_nudge > 0) {
|
||||
target_airspeed += airspeed_nudge;
|
||||
if (control_mode >= AUTO && airspeed_nudge_cm > 0) {
|
||||
target_airspeed_cm += airspeed_nudge_cm;
|
||||
}
|
||||
|
||||
// Apply airspeed limit
|
||||
if (target_airspeed > (g.flybywire_airspeed_max * 100))
|
||||
target_airspeed = (g.flybywire_airspeed_max * 100);
|
||||
if (target_airspeed_cm > (g.flybywire_airspeed_max * 100))
|
||||
target_airspeed_cm = (g.flybywire_airspeed_max * 100);
|
||||
|
||||
airspeed_error = target_airspeed - aspeed_cm;
|
||||
airspeed_energy_error = ((target_airspeed * target_airspeed) - (aspeed_cm*aspeed_cm))*0.00005;
|
||||
airspeed_error_cm = target_airspeed_cm - aspeed_cm;
|
||||
airspeed_energy_error = ((target_airspeed_cm * target_airspeed_cm) - (aspeed_cm*aspeed_cm))*0.00005;
|
||||
}
|
||||
|
||||
static void calc_gndspeed_undershoot()
|
||||
{
|
||||
// Function is overkill, but here in case we want to add filtering later
|
||||
groundspeed_undershoot = (g.min_gndspeed > 0) ? (g.min_gndspeed - g_gps->ground_speed) : 0;
|
||||
groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - g_gps->ground_speed) : 0;
|
||||
}
|
||||
|
||||
static void calc_bearing_error()
|
||||
@ -117,44 +117,44 @@ static void calc_bearing_error()
|
||||
correction using the GPS typically takes 10 seconds or so
|
||||
for a 180 degree correction.
|
||||
*/
|
||||
bearing_error = nav_bearing - ahrs.yaw_sensor;
|
||||
bearing_error_cd = nav_bearing_cd - ahrs.yaw_sensor;
|
||||
} else {
|
||||
|
||||
// TODO: we need to use the Yaw gyro for in between GPS reads,
|
||||
// maybe as an offset from a saved gryo value.
|
||||
bearing_error = nav_bearing - g_gps->ground_course;
|
||||
bearing_error_cd = nav_bearing_cd - g_gps->ground_course;
|
||||
}
|
||||
|
||||
bearing_error = wrap_180(bearing_error);
|
||||
bearing_error_cd = wrap_180_cd(bearing_error_cd);
|
||||
}
|
||||
|
||||
static void calc_altitude_error()
|
||||
{
|
||||
if(control_mode == AUTO && offset_altitude != 0) {
|
||||
if(control_mode == AUTO && offset_altitude_cm != 0) {
|
||||
// limit climb rates
|
||||
target_altitude = next_WP.alt - ((float)((wp_distance -30) * offset_altitude) / (float)(wp_totalDistance - 30));
|
||||
target_altitude_cm = next_WP.alt - ((float)((wp_distance -30) * offset_altitude_cm) / (float)(wp_totalDistance - 30));
|
||||
|
||||
// stay within a certain range
|
||||
if(prev_WP.alt > next_WP.alt){
|
||||
target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt);
|
||||
target_altitude_cm = constrain(target_altitude_cm, next_WP.alt, prev_WP.alt);
|
||||
}else{
|
||||
target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
|
||||
target_altitude_cm = constrain(target_altitude_cm, prev_WP.alt, next_WP.alt);
|
||||
}
|
||||
} else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) {
|
||||
target_altitude = next_WP.alt;
|
||||
target_altitude_cm = next_WP.alt;
|
||||
}
|
||||
|
||||
altitude_error = target_altitude - current_loc.alt;
|
||||
altitude_error_cm = target_altitude_cm - current_loc.alt;
|
||||
}
|
||||
|
||||
static long wrap_360(long error)
|
||||
static int32_t wrap_360_cd(int32_t error)
|
||||
{
|
||||
if (error > 36000) error -= 36000;
|
||||
if (error < 0) error += 36000;
|
||||
return error;
|
||||
}
|
||||
|
||||
static long wrap_180(long error)
|
||||
static int32_t wrap_180_cd(int32_t error)
|
||||
{
|
||||
if (error > 18000) error -= 36000;
|
||||
if (error < -18000) error += 36000;
|
||||
@ -168,15 +168,14 @@ static void update_loiter()
|
||||
if(wp_distance <= g.loiter_radius){
|
||||
power = float(wp_distance) / float(g.loiter_radius);
|
||||
power = constrain(power, 0.5, 1);
|
||||
nav_bearing += (int)(9000.0 * (2.0 + power));
|
||||
}else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){
|
||||
nav_bearing_cd += 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.5, 1); //power = constrain(power, 0, 1);
|
||||
nav_bearing -= power * 9000;
|
||||
|
||||
}else{
|
||||
nav_bearing_cd -= power * 9000;
|
||||
} else{
|
||||
update_crosstrack();
|
||||
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
|
||||
loiter_time_ms = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
|
||||
|
||||
}
|
||||
/*
|
||||
@ -188,22 +187,23 @@ static void update_loiter()
|
||||
|
||||
update_crosstrack();
|
||||
*/
|
||||
nav_bearing = wrap_360(nav_bearing);
|
||||
nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
|
||||
}
|
||||
|
||||
static void update_crosstrack(void)
|
||||
{
|
||||
// Crosstrack Error
|
||||
// ----------------
|
||||
if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following
|
||||
crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line
|
||||
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
|
||||
nav_bearing = wrap_360(nav_bearing);
|
||||
// If we are too far off or too close we don't do track following
|
||||
if (abs(wrap_180_cd(target_bearing_cd - crosstrack_bearing_cd)) < 4500) {
|
||||
crosstrack_error = sin(radians((target_bearing_cd - crosstrack_bearing_cd) * 0.01)) * wp_distance; // Meters we are off track line
|
||||
nav_bearing_cd += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
|
||||
nav_bearing_cd = wrap_360_cd(nav_bearing_cd);
|
||||
}
|
||||
}
|
||||
|
||||
static void reset_crosstrack()
|
||||
{
|
||||
crosstrack_bearing = get_bearing(&prev_WP, &next_WP); // Used for track following
|
||||
crosstrack_bearing_cd = get_bearing_cd(&prev_WP, &next_WP); // Used for track following
|
||||
}
|
||||
|
||||
|
||||
@ -34,8 +34,8 @@ planner_gcs(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
int loopcount = 0;
|
||||
while (1) {
|
||||
if (millis()-fast_loopTimer > 19) {
|
||||
fast_loopTimer = millis();
|
||||
if (millis()-fast_loopTimer_ms > 19) {
|
||||
fast_loopTimer_ms = millis();
|
||||
|
||||
gcs_update();
|
||||
|
||||
|
||||
@ -101,12 +101,12 @@ static void read_radio()
|
||||
|
||||
if (g.channel_throttle.servo_out > 50) {
|
||||
if (airspeed.use()) {
|
||||
airspeed_nudge = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise_cm) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
|
||||
airspeed_nudge_cm = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise_cm) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
|
||||
} else {
|
||||
throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
|
||||
}
|
||||
} else {
|
||||
airspeed_nudge = 0;
|
||||
airspeed_nudge_cm = 0;
|
||||
throttle_nudge = 0;
|
||||
}
|
||||
|
||||
|
||||
@ -40,9 +40,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
|
||||
{"xbee", test_xbee},
|
||||
{"eedump", test_eedump},
|
||||
{"modeswitch", test_modeswitch},
|
||||
#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
|
||||
{"dipswitches", test_dipswitches},
|
||||
#endif
|
||||
|
||||
// Tests below here are for hardware sensors only present
|
||||
// when real sensors are attached or they are emulated
|
||||
@ -318,10 +315,10 @@ test_wp(uint8_t argc, const Menu::arg *argv)
|
||||
delay(1000);
|
||||
|
||||
// save the alitude above home option
|
||||
if(g.RTL_altitude < 0){
|
||||
if (g.RTL_altitude_cm < 0){
|
||||
Serial.printf_P(PSTR("Hold current altitude\n"));
|
||||
}else{
|
||||
Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100);
|
||||
Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude_cm/100);
|
||||
}
|
||||
|
||||
Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
|
||||
@ -483,10 +480,10 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
if (millis() - fast_loopTimer > 19) {
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
if (millis() - fast_loopTimer_ms > 19) {
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer_ms;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
||||
fast_loopTimer = millis();
|
||||
fast_loopTimer_ms = millis();
|
||||
|
||||
// IMU
|
||||
// ---
|
||||
@ -548,10 +545,10 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
while(1) {
|
||||
delay(20);
|
||||
if (millis() - fast_loopTimer > 19) {
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
if (millis() - fast_loopTimer_ms > 19) {
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer_ms;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
||||
fast_loopTimer = millis();
|
||||
fast_loopTimer_ms = millis();
|
||||
|
||||
// IMU
|
||||
// ---
|
||||
@ -573,7 +570,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
||||
if (compass.healthy) {
|
||||
Vector3f maggy = compass.get_offsets();
|
||||
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
|
||||
(wrap_360(ToDeg(heading) * 100)) /100,
|
||||
(wrap_360_cd(ToDeg(heading) * 100)) /100,
|
||||
(int)compass.mag_x,
|
||||
(int)compass.mag_y,
|
||||
(int)compass.mag_z,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user