diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 2d2b0776ab..bd1c059ca1 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -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; diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index c26fa3eb3e..30aa38b061 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -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; } diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index d9f780472f..1fc7c71b25 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -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, diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 4fb429353e..5f76937325 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -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); diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 2dc53c69ce..fa994e6e3b 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -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; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 321120b4c8..4d6d111425 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -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 diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index cd5b037a64..c7d438f393 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -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; } diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index ff84fc47d9..5c5e6a952a 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -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(); } diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index c219d38f39..29c47d3660 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -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) { diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde index 6f5469155f..4bf9f77eed 100644 --- a/ArduPlane/geofence.pde +++ b/ArduPlane/geofence.pde @@ -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; } diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 10aeedb9c6..6e4ff683e5 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -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 } diff --git a/ArduPlane/planner.pde b/ArduPlane/planner.pde index 1a5d8ee4af..a4d44a7aab 100644 --- a/ArduPlane/planner.pde +++ b/ArduPlane/planner.pde @@ -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(); diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 7211d77e26..27ed32ccea 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -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; } diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 27ac0ca759..f716ae6074 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -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,