mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: use specific sized data types in a lot of places
this will make the Desktop build more consistent with the real AVR build, and also with a future ARM build
This commit is contained in:
Andrew Tridgell
parent
0f3c21821a
commit
28634f51b7
|
|
@ -247,28 +247,28 @@ static const char* flight_mode_strings[] = {
|
|||
*/
|
||||
|
||||
// temp
|
||||
static int y_actual_speed;
|
||||
static int y_rate_error;
|
||||
static int16_t y_actual_speed;
|
||||
static int16_t y_rate_error;
|
||||
|
||||
// calc the
|
||||
static int x_actual_speed;
|
||||
static int x_rate_error;
|
||||
static int16_t x_actual_speed;
|
||||
static int16_t x_rate_error;
|
||||
|
||||
// Radio
|
||||
// -----
|
||||
static byte control_mode = STABILIZE;
|
||||
static byte old_control_mode = STABILIZE;
|
||||
static byte oldSwitchPosition; // for remembering the control mode switch
|
||||
static int motor_out[8];
|
||||
static int16_t motor_out[8];
|
||||
static bool do_simple = false;
|
||||
|
||||
// Heli
|
||||
// ----
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
static float heli_rollFactor[3], heli_pitchFactor[3]; // only required for 3 swashplate servos
|
||||
static int heli_servo_min[3], heli_servo_max[3]; // same here. for yaw servo we use heli_servo4_min/max parameter directly
|
||||
static long heli_servo_out[4]; // used for servo averaging for analog servos
|
||||
static int heli_servo_out_count = 0; // use for servo averaging
|
||||
static int16_t heli_servo_min[3], heli_servo_max[3]; // same here. for yaw servo we use heli_servo4_min/max parameter directly
|
||||
static int32_t heli_servo_out[4]; // used for servo averaging for analog servos
|
||||
static int16_t heli_servo_out_count = 0; // use for servo averaging
|
||||
#endif
|
||||
|
||||
// Failsafe
|
||||
|
|
@ -301,7 +301,7 @@ static bool did_ground_start = false; // have we ground started after first ar
|
|||
// ---------------------
|
||||
static const float radius_of_earth = 6378100; // meters
|
||||
static const float gravity = 9.81; // meters/ sec^2
|
||||
static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
|
||||
static int32_t target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
|
||||
|
||||
static byte wp_control; // used to control - navgation or loiter
|
||||
|
||||
|
|
@ -315,10 +315,10 @@ static float cos_roll_x = 1;
|
|||
static float cos_pitch_x = 1;
|
||||
static float cos_yaw_x = 1;
|
||||
static float sin_pitch_y, sin_yaw_y, sin_roll_y;
|
||||
static long initial_simple_bearing; // used for Simple mode
|
||||
static int32_t initial_simple_bearing; // used for Simple mode
|
||||
static float simple_sin_y, simple_cos_x;
|
||||
static byte jump = -10; // used to track loops in jump command
|
||||
static int waypoint_speed_gov;
|
||||
static int16_t waypoint_speed_gov;
|
||||
|
||||
// Acro
|
||||
#if CH7_OPTION == CH7_FLIP
|
||||
|
|
@ -326,16 +326,16 @@ static bool do_flip = false;
|
|||
#endif
|
||||
|
||||
static boolean trim_flag;
|
||||
static int CH7_wp_index = 0;
|
||||
static int16_t CH7_wp_index = 0;
|
||||
|
||||
// Airspeed
|
||||
// --------
|
||||
static int airspeed; // m/s * 100
|
||||
static int16_t airspeed; // m/s * 100
|
||||
|
||||
// Location Errors
|
||||
// ---------------
|
||||
static long yaw_error; // how off are we pointed
|
||||
static long long_error, lat_error; // temp for debugging
|
||||
static int32_t yaw_error; // how off are we pointed
|
||||
static int32_t long_error, lat_error; // temp for debugging
|
||||
|
||||
// Battery Sensors
|
||||
// ---------------
|
||||
|
|
@ -351,24 +351,24 @@ static bool low_batt = false;
|
|||
|
||||
// Barometer Sensor variables
|
||||
// --------------------------
|
||||
static long abs_pressure;
|
||||
static long ground_pressure;
|
||||
static int ground_temperature;
|
||||
static int32_t abs_pressure;
|
||||
static int32_t ground_pressure;
|
||||
static int16_t ground_temperature;
|
||||
|
||||
// Altitude Sensor variables
|
||||
// ----------------------
|
||||
static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR
|
||||
static long altitude_error; // meters * 100 we are off in altitude
|
||||
static int32_t altitude_error; // meters * 100 we are off in altitude
|
||||
|
||||
static int climb_rate; // m/s * 100
|
||||
static int16_t climb_rate; // m/s * 100
|
||||
|
||||
static int sonar_alt;
|
||||
static int old_sonar_alt;
|
||||
static int sonar_rate;
|
||||
static int16_t sonar_alt;
|
||||
static int16_t old_sonar_alt;
|
||||
static int16_t sonar_rate;
|
||||
|
||||
static int baro_alt;
|
||||
static int old_baro_alt;
|
||||
static int baro_rate;
|
||||
static int16_t baro_alt;
|
||||
static int16_t old_baro_alt;
|
||||
static int16_t baro_rate;
|
||||
|
||||
|
||||
|
||||
|
|
@ -380,75 +380,75 @@ static byte throttle_mode;
|
|||
|
||||
static boolean takeoff_complete; // Flag for using take-off controls
|
||||
static boolean land_complete;
|
||||
static long old_alt; // used for managing altitude rates
|
||||
static int velocity_land;
|
||||
static int32_t old_alt; // used for managing altitude rates
|
||||
static int16_t velocity_land;
|
||||
static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target
|
||||
static int manual_boost; // used in adjust altitude to make changing alt faster
|
||||
static int angle_boost; // used in adjust altitude to make changing alt faster
|
||||
static int16_t manual_boost; // used in adjust altitude to make changing alt faster
|
||||
static int16_t angle_boost; // used in adjust altitude to make changing alt faster
|
||||
|
||||
// Loiter management
|
||||
// -----------------
|
||||
static long original_target_bearing; // deg * 100, used to check we are not passing the WP
|
||||
static long old_target_bearing; // used to track difference in angle
|
||||
static int32_t original_target_bearing; // deg * 100, used to check we are not passing the WP
|
||||
static int32_t old_target_bearing; // used to track difference in angle
|
||||
|
||||
static int loiter_total; // deg : how many times to loiter * 360
|
||||
static int loiter_sum; // deg : how far we have turned around a waypoint
|
||||
static unsigned long loiter_time; // millis : when we started LOITER mode
|
||||
static int16_t loiter_total; // deg : how many times to loiter * 360
|
||||
static int16_t loiter_sum; // deg : how far we have turned around a waypoint
|
||||
static uint32_t loiter_time; // millis : when we started LOITER mode
|
||||
static unsigned loiter_time_max; // millis : how long to stay in LOITER mode
|
||||
|
||||
|
||||
// these are the values for navigation control functions
|
||||
// ----------------------------------------------------
|
||||
static long nav_roll; // deg * 100 : target roll angle
|
||||
static long nav_pitch; // deg * 100 : target pitch angle
|
||||
static long nav_yaw; // deg * 100 : target yaw angle
|
||||
static long auto_yaw; // deg * 100 : target yaw angle
|
||||
static long nav_lat; // for error calcs
|
||||
static long nav_lon; // for error calcs
|
||||
static int nav_throttle; // 0-1000 for throttle control
|
||||
static int32_t nav_roll; // deg * 100 : target roll angle
|
||||
static int32_t nav_pitch; // deg * 100 : target pitch angle
|
||||
static int32_t nav_yaw; // deg * 100 : target yaw angle
|
||||
static int32_t auto_yaw; // deg * 100 : target yaw angle
|
||||
static int32_t nav_lat; // for error calcs
|
||||
static int32_t nav_lon; // for error calcs
|
||||
static int16_t nav_throttle; // 0-1000 for throttle control
|
||||
|
||||
static unsigned long throttle_integrator; // used to integrate throttle output to predict battery life
|
||||
static uint32_t throttle_integrator; // used to integrate throttle output to predict battery life
|
||||
static bool invalid_throttle; // used to control when we calculate nav_throttle
|
||||
//static bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
|
||||
|
||||
static long command_yaw_start; // what angle were we to begin with
|
||||
static unsigned long command_yaw_start_time; // when did we start turning
|
||||
static unsigned int command_yaw_time; // how long we are turning
|
||||
static long command_yaw_end; // what angle are we trying to be
|
||||
static long command_yaw_delta; // how many degrees will we turn
|
||||
static int command_yaw_speed; // how fast to turn
|
||||
static int32_t command_yaw_start; // what angle were we to begin with
|
||||
static uint32_t command_yaw_start_time; // when did we start turning
|
||||
static uint16_t command_yaw_time; // how long we are turning
|
||||
static int32_t command_yaw_end; // what angle are we trying to be
|
||||
static int32_t command_yaw_delta; // how many degrees will we turn
|
||||
static int16_t command_yaw_speed; // how fast to turn
|
||||
static byte command_yaw_dir;
|
||||
static byte command_yaw_relative;
|
||||
|
||||
static int auto_level_counter;
|
||||
static int16_t auto_level_counter;
|
||||
|
||||
// Waypoints
|
||||
// ---------
|
||||
static long wp_distance; // meters - distance between plane and next waypoint
|
||||
static long wp_totalDistance; // meters - distance between old and next waypoint
|
||||
static int32_t wp_distance; // meters - distance between plane and next waypoint
|
||||
static int32_t wp_totalDistance; // meters - distance between old and next waypoint
|
||||
//static byte next_wp_index; // Current active command index
|
||||
|
||||
// repeating event control
|
||||
// -----------------------
|
||||
static byte event_id; // what to do - see defines
|
||||
static unsigned long event_timer; // when the event was asked for in ms
|
||||
static unsigned int event_delay; // how long to delay the next firing of event in millis
|
||||
static int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice
|
||||
static int event_value; // per command value, such as PWM for servos
|
||||
static int event_undo_value; // the value used to undo commands
|
||||
static byte event_id; // what to do - see defines
|
||||
static uint32_t event_timer; // when the event was asked for in ms
|
||||
static uint16_t event_delay; // how long to delay the next firing of event in millis
|
||||
static int16_t event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice
|
||||
static int16_t event_value; // per command value, such as PWM for servos
|
||||
static int16_t event_undo_value; // the value used to undo commands
|
||||
//static byte repeat_forever;
|
||||
//static byte undo_event; // counter for timing the undo
|
||||
|
||||
// delay command
|
||||
// --------------
|
||||
static long condition_value; // used in condition commands (eg delay, change alt, etc.)
|
||||
static long condition_start;
|
||||
//static int condition_rate;
|
||||
static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
|
||||
static int32_t condition_start;
|
||||
//static int16_t condition_rate;
|
||||
|
||||
// land command
|
||||
// ------------
|
||||
static long land_start; // when we intiated command in millis()
|
||||
static long original_alt; // altitide reference for start of command
|
||||
static int32_t land_start; // when we intiated command in millis()
|
||||
static int32_t original_alt; // altitide reference for start of command
|
||||
|
||||
// 3D Location vectors
|
||||
// -------------------
|
||||
|
|
@ -459,7 +459,7 @@ static struct Location next_WP; // next waypoint
|
|||
static struct Location target_WP; // where do we want to you towards?
|
||||
static struct Location next_command; // command preloaded
|
||||
static struct Location guided_WP; // guided mode waypoint
|
||||
static long target_altitude; // used for
|
||||
static int32_t target_altitude; // used for
|
||||
static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location
|
||||
|
||||
// IMU variables
|
||||
|
|
@ -468,25 +468,25 @@ static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm
|
|||
|
||||
// Performance monitoring
|
||||
// ----------------------
|
||||
static long perf_mon_timer;
|
||||
static int32_t perf_mon_timer;
|
||||
//static float imu_health; // Metric based on accel gain deweighting
|
||||
static int gps_fix_count;
|
||||
static int16_t gps_fix_count;
|
||||
static byte gps_watchdog;
|
||||
|
||||
// System Timers
|
||||
// --------------
|
||||
static unsigned long fast_loopTimer; // Time in miliseconds of main control loop
|
||||
static uint32_t fast_loopTimer; // Time in miliseconds of main control loop
|
||||
static byte medium_loopCounter; // Counters for branching from main control loop to slower loops
|
||||
|
||||
static unsigned long fiftyhz_loopTimer;
|
||||
static uint32_t fiftyhz_loopTimer;
|
||||
|
||||
static byte slow_loopCounter;
|
||||
static int superslow_loopCounter;
|
||||
static int16_t superslow_loopCounter;
|
||||
static byte simple_timer; // for limiting the execution of flight mode thingys
|
||||
|
||||
|
||||
static float dTnav; // Delta Time in milliseconds for navigation computations
|
||||
static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav
|
||||
static uint32_t nav_loopTimer; // used to track the elapsed ime for GPS nav
|
||||
|
||||
static byte counter_one_herz;
|
||||
static bool GPS_enabled = false;
|
||||
|
|
@ -505,7 +505,7 @@ void setup() {
|
|||
|
||||
void loop()
|
||||
{
|
||||
long timer = micros();
|
||||
int32_t timer = micros();
|
||||
// We want this to execute fast
|
||||
// ----------------------------
|
||||
if ((timer - fast_loopTimer) >= 5000) {
|
||||
|
|
|
|||
|
|
@ -1,9 +1,9 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
static int
|
||||
get_stabilize_roll(long target_angle)
|
||||
get_stabilize_roll(int32_t target_angle)
|
||||
{
|
||||
long error;
|
||||
long rate;
|
||||
int32_t error;
|
||||
int32_t rate;
|
||||
|
||||
error = wrap_180(target_angle - dcm.roll_sensor);
|
||||
|
||||
|
|
@ -16,7 +16,7 @@ get_stabilize_roll(long target_angle)
|
|||
|
||||
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
|
||||
// Rate P:
|
||||
error = rate - (long)(degrees(omega.x) * 100.0);
|
||||
error = rate - (degrees(omega.x) * 100.0);
|
||||
rate = g.pi_rate_roll.get_pi(error, G_Dt);
|
||||
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
|
||||
#endif
|
||||
|
|
@ -26,10 +26,10 @@ get_stabilize_roll(long target_angle)
|
|||
}
|
||||
|
||||
static int
|
||||
get_stabilize_pitch(long target_angle)
|
||||
get_stabilize_pitch(int32_t target_angle)
|
||||
{
|
||||
long error;
|
||||
long rate;
|
||||
int32_t error;
|
||||
int32_t rate;
|
||||
|
||||
error = wrap_180(target_angle - dcm.pitch_sensor);
|
||||
|
||||
|
|
@ -42,7 +42,7 @@ get_stabilize_pitch(long target_angle)
|
|||
|
||||
#if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters
|
||||
// Rate P:
|
||||
error = rate - (long)(degrees(omega.y) * 100.0);
|
||||
error = rate - (degrees(omega.y) * 100.0);
|
||||
rate = g.pi_rate_pitch.get_pi(error, G_Dt);
|
||||
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
|
||||
#endif
|
||||
|
|
@ -54,10 +54,10 @@ get_stabilize_pitch(long target_angle)
|
|||
|
||||
#define YAW_ERROR_MAX 2000
|
||||
static int
|
||||
get_stabilize_yaw(long target_angle)
|
||||
get_stabilize_yaw(int32_t target_angle)
|
||||
{
|
||||
long error;
|
||||
long rate;
|
||||
int32_t error;
|
||||
int32_t rate;
|
||||
|
||||
yaw_error = wrap_180(target_angle - dcm.yaw_sensor);
|
||||
|
||||
|
|
@ -69,7 +69,7 @@ get_stabilize_yaw(long target_angle)
|
|||
#if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters
|
||||
if( ! g.heli_ext_gyro_enabled ) {
|
||||
// Rate P:
|
||||
error = rate - (long)(degrees(omega.z) * 100.0);
|
||||
error = rate - (degrees(omega.z) * 100.0);
|
||||
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
||||
}
|
||||
|
||||
|
|
@ -77,7 +77,7 @@ get_stabilize_yaw(long target_angle)
|
|||
return (int)constrain(rate, -4500, 4500);
|
||||
#else
|
||||
// Rate P:
|
||||
error = rate - (long)(degrees(omega.z) * 100.0);
|
||||
error = rate - (degrees(omega.z) * 100.0);
|
||||
rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
||||
//Serial.printf("%d\t%d\n", (int)error, (int)rate);
|
||||
|
||||
|
|
@ -89,7 +89,7 @@ get_stabilize_yaw(long target_angle)
|
|||
|
||||
#define ALT_ERROR_MAX 300
|
||||
static int
|
||||
get_nav_throttle(long z_error)
|
||||
get_nav_throttle(int32_t z_error)
|
||||
{
|
||||
bool calc_i = abs(z_error) < ALT_ERROR_MAX;
|
||||
// limit error to prevent I term run up
|
||||
|
|
@ -102,24 +102,24 @@ get_nav_throttle(long z_error)
|
|||
}
|
||||
|
||||
static int
|
||||
get_rate_roll(long target_rate)
|
||||
get_rate_roll(int32_t target_rate)
|
||||
{
|
||||
long error = (target_rate * 3.5) - (long)(degrees(omega.x) * 100.0);
|
||||
int32_t error = (target_rate * 3.5) - (degrees(omega.x) * 100.0);
|
||||
return g.pi_acro_roll.get_pi(error, G_Dt);
|
||||
}
|
||||
|
||||
static int
|
||||
get_rate_pitch(long target_rate)
|
||||
get_rate_pitch(int32_t target_rate)
|
||||
{
|
||||
long error = (target_rate * 3.5) - (long)(degrees(omega.y) * 100.0);
|
||||
int32_t error = (target_rate * 3.5) - (degrees(omega.y) * 100.0);
|
||||
return g.pi_acro_pitch.get_pi(error, G_Dt);
|
||||
}
|
||||
|
||||
static int
|
||||
get_rate_yaw(long target_rate)
|
||||
get_rate_yaw(int32_t target_rate)
|
||||
{
|
||||
long error;
|
||||
error = (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0);
|
||||
int32_t error;
|
||||
error = (target_rate * 4.5) - (degrees(omega.z) * 100.0);
|
||||
target_rate = g.pi_rate_yaw.get_pi(error, G_Dt);
|
||||
|
||||
// output control:
|
||||
|
|
@ -158,7 +158,7 @@ throttle control
|
|||
static long
|
||||
get_nav_yaw_offset(int yaw_input, int reset)
|
||||
{
|
||||
long _yaw;
|
||||
int32_t _yaw;
|
||||
|
||||
if(reset == 0){
|
||||
// we are on the ground
|
||||
|
|
@ -168,7 +168,7 @@ get_nav_yaw_offset(int yaw_input, int reset)
|
|||
// re-define nav_yaw if we have stick input
|
||||
if(yaw_input != 0){
|
||||
// set nav_yaw + or - the current location
|
||||
_yaw = (long)yaw_input + dcm.yaw_sensor;
|
||||
_yaw = yaw_input + dcm.yaw_sensor;
|
||||
// we need to wrap our value so we can be 0 to 360 (*100)
|
||||
return wrap_360(_yaw);
|
||||
|
||||
|
|
|
|||
|
|
@ -1660,8 +1660,8 @@ GCS_MAVLINK::queued_waypoint_send()
|
|||
*/
|
||||
static void mavlink_delay(unsigned long t)
|
||||
{
|
||||
unsigned long tstart;
|
||||
static unsigned long last_1hz, last_50hz;
|
||||
uint32_t tstart;
|
||||
static uint32_t last_1hz, last_50hz;
|
||||
|
||||
if (in_mavlink_delay) {
|
||||
// this should never happen, but let's not tempt fate by
|
||||
|
|
@ -1674,7 +1674,7 @@ static void mavlink_delay(unsigned long t)
|
|||
|
||||
tstart = millis();
|
||||
do {
|
||||
unsigned long tnow = millis();
|
||||
uint32_t tnow = millis();
|
||||
if (tnow - last_1hz > 1000) {
|
||||
last_1hz = tnow;
|
||||
gcs_send_message(MSG_HEARTBEAT);
|
||||
|
|
|
|||
|
|
@ -384,12 +384,12 @@ static void Log_Read_GPS()
|
|||
"%d, %u\n"),
|
||||
|
||||
DataFlash.ReadLong(), // 1 time
|
||||
(int)DataFlash.ReadByte(), // 2 sats
|
||||
DataFlash.ReadByte(), // 2 sats
|
||||
|
||||
(float)DataFlash.ReadLong() / t7, // 3 lat
|
||||
(float)DataFlash.ReadLong() / t7, // 4 lon
|
||||
(float)DataFlash.ReadLong() / 100.0, // 5 gps alt
|
||||
(float)DataFlash.ReadLong() / 100.0, // 6 sensor alt
|
||||
DataFlash.ReadLong() / t7, // 3 lat
|
||||
DataFlash.ReadLong() / t7, // 4 lon
|
||||
DataFlash.ReadLong() / 100.0, // 5 gps alt
|
||||
DataFlash.ReadLong() / 100.0, // 6 sensor alt
|
||||
|
||||
DataFlash.ReadInt(), // 7 ground speed
|
||||
(uint16_t)DataFlash.ReadInt()); // 8 ground course
|
||||
|
|
@ -411,18 +411,18 @@ static void Log_Write_Raw()
|
|||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_RAW_MSG);
|
||||
|
||||
DataFlash.WriteLong((long)gyro.x);
|
||||
DataFlash.WriteLong((long)gyro.y);
|
||||
DataFlash.WriteLong((long)gyro.z);
|
||||
DataFlash.WriteLong(gyro.x);
|
||||
DataFlash.WriteLong(gyro.y);
|
||||
DataFlash.WriteLong(gyro.z);
|
||||
|
||||
|
||||
//DataFlash.WriteLong((long)(accels_rot.x * t7));
|
||||
//DataFlash.WriteLong((long)(accels_rot.y * t7));
|
||||
//DataFlash.WriteLong((long)(accels_rot.z * t7));
|
||||
//DataFlash.WriteLong(accels_rot.x * t7);
|
||||
//DataFlash.WriteLong(accels_rot.y * t7);
|
||||
//DataFlash.WriteLong(accels_rot.z * t7);
|
||||
|
||||
DataFlash.WriteLong((long)accel.x);
|
||||
DataFlash.WriteLong((long)accel.y);
|
||||
DataFlash.WriteLong((long)accel.z);
|
||||
DataFlash.WriteLong(accel.x);
|
||||
DataFlash.WriteLong(accel.y);
|
||||
DataFlash.WriteLong(accel.z);
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
|
@ -450,9 +450,9 @@ static void Log_Write_Current()
|
|||
DataFlash.WriteInt(g.rc_3.control_in);
|
||||
DataFlash.WriteLong(throttle_integrator);
|
||||
|
||||
DataFlash.WriteInt((int)(battery_voltage * 100.0));
|
||||
DataFlash.WriteInt((int)(current_amps * 100.0));
|
||||
DataFlash.WriteInt((int)current_total);
|
||||
DataFlash.WriteInt(battery_voltage * 100.0);
|
||||
DataFlash.WriteInt(current_amps * 100.0);
|
||||
DataFlash.WriteInt(current_total);
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
|
@ -464,8 +464,8 @@ static void Log_Read_Current()
|
|||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadLong(),
|
||||
|
||||
((float)DataFlash.ReadInt() / 100.f),
|
||||
((float)DataFlash.ReadInt() / 100.f),
|
||||
(DataFlash.ReadInt() / 100.f),
|
||||
(DataFlash.ReadInt() / 100.f),
|
||||
DataFlash.ReadInt());
|
||||
}
|
||||
|
||||
|
|
@ -608,16 +608,16 @@ static void Log_Write_Nav_Tuning()
|
|||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
|
||||
|
||||
DataFlash.WriteInt((int)wp_distance); // 1
|
||||
DataFlash.WriteInt((int)(target_bearing/100)); // 2
|
||||
DataFlash.WriteInt((int)long_error); // 3
|
||||
DataFlash.WriteInt((int)lat_error); // 4
|
||||
DataFlash.WriteInt((int)nav_lon); // 5
|
||||
DataFlash.WriteInt((int)nav_lat); // 6
|
||||
DataFlash.WriteInt((int)g.pi_nav_lon.get_integrator()); // 7
|
||||
DataFlash.WriteInt((int)g.pi_nav_lat.get_integrator()); // 8
|
||||
DataFlash.WriteInt((int)g.pi_loiter_lon.get_integrator()); // 9
|
||||
DataFlash.WriteInt((int)g.pi_loiter_lat.get_integrator()); // 10
|
||||
DataFlash.WriteInt(wp_distance); // 1
|
||||
DataFlash.WriteInt(target_bearing/100); // 2
|
||||
DataFlash.WriteInt(long_error); // 3
|
||||
DataFlash.WriteInt(lat_error); // 4
|
||||
DataFlash.WriteInt(nav_lon); // 5
|
||||
DataFlash.WriteInt(nav_lat); // 6
|
||||
DataFlash.WriteInt(g.pi_nav_lon.get_integrator()); // 7
|
||||
DataFlash.WriteInt(g.pi_nav_lat.get_integrator()); // 8
|
||||
DataFlash.WriteInt(g.pi_loiter_lon.get_integrator()); // 9
|
||||
DataFlash.WriteInt(g.pi_loiter_lat.get_integrator()); // 10
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
|
@ -648,14 +648,14 @@ static void Log_Write_Control_Tuning()
|
|||
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
|
||||
|
||||
// yaw
|
||||
DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); //1
|
||||
DataFlash.WriteInt((int)(nav_yaw/100)); //2
|
||||
DataFlash.WriteInt((int)yaw_error/100); //3
|
||||
DataFlash.WriteInt(dcm.yaw_sensor/100); //1
|
||||
DataFlash.WriteInt(nav_yaw/100); //2
|
||||
DataFlash.WriteInt(yaw_error/100); //3
|
||||
|
||||
// Alt hold
|
||||
DataFlash.WriteInt(sonar_alt); //4
|
||||
DataFlash.WriteInt(baro_alt); //5
|
||||
DataFlash.WriteInt((int)next_WP.alt); //6
|
||||
DataFlash.WriteInt(next_WP.alt); //6
|
||||
|
||||
DataFlash.WriteInt(nav_throttle); //7
|
||||
DataFlash.WriteInt(angle_boost); //8
|
||||
|
|
@ -669,8 +669,8 @@ static void Log_Write_Control_Tuning()
|
|||
#endif
|
||||
|
||||
DataFlash.WriteInt(g.rc_3.servo_out); //11
|
||||
DataFlash.WriteInt((int)g.pi_alt_hold.get_integrator()); //12
|
||||
DataFlash.WriteInt((int)g.pi_throttle.get_integrator()); //13
|
||||
DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); //12
|
||||
DataFlash.WriteInt(g.pi_throttle.get_integrator()); //13
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
|
@ -813,13 +813,13 @@ static void Log_Write_Attitude2()
|
|||
DataFlash.WriteInt((int)dcm.roll_sensor);
|
||||
DataFlash.WriteInt((int)dcm.pitch_sensor);
|
||||
|
||||
DataFlash.WriteLong((long)(degrees(omega.x) * 100.0));
|
||||
DataFlash.WriteLong((long)(degrees(omega.y) * 100.0));
|
||||
DataFlash.WriteLong(degrees(omega.x) * 100.0);
|
||||
DataFlash.WriteLong(degrees(omega.y) * 100.0);
|
||||
|
||||
DataFlash.WriteLong((long)(accel.x * 100000));
|
||||
DataFlash.WriteLong((long)(accel.y * 100000));
|
||||
DataFlash.WriteLong(accel.x * 100000);
|
||||
DataFlash.WriteLong(accel.y * 100000);
|
||||
|
||||
//DataFlash.WriteLong((long)(accel.z * 100000));
|
||||
//DataFlash.WriteLong(accel.z * 100000);
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}*/
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ static struct Location get_cmd_with_index(int i)
|
|||
}else{
|
||||
// we can load a command, we don't process it yet
|
||||
// read WP position
|
||||
long mem = (WP_START_BYTE) + (i * WP_SIZE);
|
||||
int32_t mem = (WP_START_BYTE) + (i * WP_SIZE);
|
||||
|
||||
temp.id = eeprom_read_byte((uint8_t*)mem);
|
||||
|
||||
|
|
@ -50,13 +50,13 @@ static struct Location get_cmd_with_index(int i)
|
|||
temp.p1 = eeprom_read_byte((uint8_t*)mem);
|
||||
|
||||
mem++;
|
||||
temp.alt = (long)eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! Alt is stored relative!
|
||||
temp.alt = eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! Alt is stored relative!
|
||||
|
||||
mem += 4;
|
||||
temp.lat = (long)eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000
|
||||
temp.lat = eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000
|
||||
|
||||
mem += 4;
|
||||
temp.lng = (long)eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
|
||||
temp.lng = eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
|
||||
}
|
||||
|
||||
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
|
||||
|
|
@ -115,7 +115,7 @@ static void decrement_WP_index()
|
|||
}
|
||||
}*/
|
||||
|
||||
static long read_alt_to_hold()
|
||||
static int32_t read_alt_to_hold()
|
||||
{
|
||||
if(g.RTL_altitude < 0)
|
||||
return current_loc.alt;
|
||||
|
|
|
|||
|
|
@ -28,7 +28,7 @@ static byte navigate()
|
|||
|
||||
static bool check_missed_wp()
|
||||
{
|
||||
long temp = target_bearing - original_target_bearing;
|
||||
int32_t temp = target_bearing - original_target_bearing;
|
||||
temp = wrap_180(temp);
|
||||
return (abs(temp) > 10000); //we pased the waypoint by 10 °
|
||||
}
|
||||
|
|
@ -205,7 +205,7 @@ static void calc_nav_pitch_roll()
|
|||
nav_pitch);*/
|
||||
}
|
||||
|
||||
static long get_altitude_error()
|
||||
static int32_t get_altitude_error()
|
||||
{
|
||||
return next_WP.alt - current_loc.alt;
|
||||
}
|
||||
|
|
@ -228,14 +228,14 @@ static int get_loiter_angle()
|
|||
return angle;
|
||||
}
|
||||
|
||||
static long wrap_360(long error)
|
||||
static int32_t wrap_360(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(int32_t error)
|
||||
{
|
||||
if (error > 18000) error -= 36000;
|
||||
if (error < -18000) error += 36000;
|
||||
|
|
@ -243,7 +243,7 @@ static long wrap_180(long error)
|
|||
}
|
||||
|
||||
/*
|
||||
static long get_crosstrack_correction(void)
|
||||
static int32_t get_crosstrack_correction(void)
|
||||
{
|
||||
// Crosstrack Error
|
||||
// ----------------
|
||||
|
|
@ -253,7 +253,7 @@ static long get_crosstrack_correction(void)
|
|||
float error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance;
|
||||
|
||||
// take meters * 100 to get adjustment to nav_bearing
|
||||
long _crosstrack_correction = g.pi_crosstrack.get_pi(error, dTnav) * 100;
|
||||
int32_t _crosstrack_correction = g.pi_crosstrack.get_pi(error, dTnav) * 100;
|
||||
|
||||
// constrain answer to 30° to avoid overshoot
|
||||
return constrain(_crosstrack_correction, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
|
||||
|
|
@ -262,9 +262,9 @@ static long get_crosstrack_correction(void)
|
|||
}
|
||||
*/
|
||||
/*
|
||||
static long cross_track_test()
|
||||
static int32_t cross_track_test()
|
||||
{
|
||||
long temp = wrap_180(target_bearing - crosstrack_bearing);
|
||||
int32_t temp = wrap_180(target_bearing - crosstrack_bearing);
|
||||
return abs(temp);
|
||||
}
|
||||
*/
|
||||
|
|
@ -274,7 +274,7 @@ static void reset_crosstrack()
|
|||
crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following
|
||||
}
|
||||
*/
|
||||
/*static long get_altitude_above_home(void)
|
||||
/*static int32_t get_altitude_above_home(void)
|
||||
{
|
||||
// This is the altitude above the home location
|
||||
// The GPS gives us altitude at Sea Level
|
||||
|
|
@ -284,7 +284,7 @@ static void reset_crosstrack()
|
|||
}
|
||||
*/
|
||||
// distance is returned in meters
|
||||
static long get_distance(struct Location *loc1, struct Location *loc2)
|
||||
static int32_t get_distance(struct Location *loc1, struct Location *loc2)
|
||||
{
|
||||
//if(loc1->lat == 0 || loc1->lng == 0)
|
||||
// return -1;
|
||||
|
|
@ -295,16 +295,16 @@ static long get_distance(struct Location *loc1, struct Location *loc2)
|
|||
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
|
||||
}
|
||||
/*
|
||||
static long get_alt_distance(struct Location *loc1, struct Location *loc2)
|
||||
static int32_t get_alt_distance(struct Location *loc1, struct Location *loc2)
|
||||
{
|
||||
return abs(loc1->alt - loc2->alt);
|
||||
}
|
||||
*/
|
||||
static long get_bearing(struct Location *loc1, struct Location *loc2)
|
||||
static int32_t get_bearing(struct Location *loc1, struct Location *loc2)
|
||||
{
|
||||
long off_x = loc2->lng - loc1->lng;
|
||||
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
|
||||
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
|
||||
int32_t off_x = loc2->lng - loc1->lng;
|
||||
int32_t off_y = (loc2->lat - loc1->lat) * scaleLongUp;
|
||||
int32_t bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
|
||||
if (bearing < 0) bearing += 36000;
|
||||
return bearing;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@ static void init_barometer(void)
|
|||
}
|
||||
|
||||
/*
|
||||
static long read_baro_filtered(void)
|
||||
static int32_t read_baro_filtered(void)
|
||||
{
|
||||
|
||||
// get new data from absolute pressure sensor
|
||||
|
|
@ -53,7 +53,7 @@ static long read_baro_filtered(void)
|
|||
|
||||
return barometer.Press;
|
||||
|
||||
long pressure = 0;
|
||||
int32_t pressure = 0;
|
||||
// add new data into our filter
|
||||
baro_filter[baro_filter_index] = barometer.Press;
|
||||
baro_filter_index++;
|
||||
|
|
@ -75,7 +75,7 @@ static long read_baro_filtered(void)
|
|||
//
|
||||
}
|
||||
*/
|
||||
static long read_barometer(void)
|
||||
static int32_t read_barometer(void)
|
||||
{
|
||||
float x, scaling, temp;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue