From 87d558104611cf6ac1c38c5fb18d7aa0f5c4c560 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 8 Nov 2011 09:24:32 +1100 Subject: [PATCH] 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 --- ArduCopter/ArduCopter.pde | 142 ++++++++++++++++++------------------- ArduCopter/Attitude.pde | 46 ++++++------ ArduCopter/GCS_Mavlink.pde | 6 +- ArduCopter/Log.pde | 80 ++++++++++----------- ArduCopter/commands.pde | 10 +-- ArduCopter/navigation.pde | 30 ++++---- ArduCopter/sensors.pde | 6 +- 7 files changed, 160 insertions(+), 160 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 7ee7c70c7f..bbd6ae8df6 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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) { diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 39a182e980..e93dfda28d 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -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); diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 663c10aeac..afdd6f5309 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -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); diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 79f79cd420..0a717e6a7c 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -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); }*/ diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index f8af4f2361..0c11437810 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -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; diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 1689d84d4c..97cc10950c 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -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; } diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 12e0647aba..b7da236f8a 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -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;