diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 78412e7a9c..8d88f24afa 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -906,8 +906,6 @@ static byte medium_loopCounter; static byte slow_loopCounter; // Counters for branching at 1 hz static byte counter_one_herz; -// Stat machine counter for Simple Mode -static byte simple_counter; // used to track the elapsed time between GPS reads static uint32_t nav_loopTimer; // Delta Time in milliseconds for navigation computations, updated with every good GPS read @@ -1609,7 +1607,7 @@ void update_yaw_mode(void) void update_roll_pitch_mode(void) { - int control_roll, control_pitch; + int16_t control_roll, control_pitch; if (do_flip){ if(abs(g.rc_1.control_in) < 4000){ @@ -1719,13 +1717,14 @@ void update_roll_pitch_mode(void) // new radio frame is used to make sure we only call this at 50hz void update_simple_mode(void) { + static byte simple_counter = 0; // State machine counter for Simple Mode static float simple_sin_y=0, simple_cos_x=0; // used to manage state machine // which improves speed of function simple_counter++; - int delta = wrap_360(ahrs.yaw_sensor - initial_simple_bearing)/100; + int16_t delta = wrap_360(ahrs.yaw_sensor - initial_simple_bearing)/100; if (simple_counter == 1){ // roll @@ -1738,8 +1737,8 @@ void update_simple_mode(void) } // Rotate input by the initial bearing - int control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y; - int control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x); + int16_t control_roll = g.rc_1.control_in * simple_cos_x + g.rc_2.control_in * simple_sin_y; + int16_t control_pitch = -(g.rc_1.control_in * simple_sin_y - g.rc_2.control_in * simple_cos_x); g.rc_1.control_in = control_roll; g.rc_2.control_in = control_pitch; @@ -2089,7 +2088,7 @@ static void update_altitude() #if HIL_MODE == HIL_MODE_ATTITUDE static int32_t old_baro_alt = 0; // we are in the SIM, fake out the baro and Sonar - int fake_relative_alt = g_gps->altitude - gps_base_alt; + int16_t fake_relative_alt = g_gps->altitude - gps_base_alt; baro_alt = fake_relative_alt; sonar_alt = fake_relative_alt; @@ -2109,14 +2108,14 @@ static void update_altitude() // calc the vertical accel rate /* // 2.6 way - int temp = (baro_alt - old_baro_alt) * 10; + int16_t temp = (baro_alt - old_baro_alt) * 10; baro_rate = (temp + baro_rate) >> 1; baro_rate = constrain(baro_rate, -300, 300); old_baro_alt = baro_alt; */ // Using Tridge's new clamb rate calc - int temp = barometer.get_climb_rate() * 100; + int16_t temp = barometer.get_climb_rate() * 100; baro_rate = (temp + baro_rate) >> 1; baro_rate = constrain(baro_rate, -300, 300); #endif @@ -2382,7 +2381,7 @@ static void update_nav_wp() }else if(wp_control == CIRCLE_MODE){ // check if we have missed the WP - int loiter_delta = (target_bearing - old_target_bearing)/100; + int16_t loiter_delta = (target_bearing - old_target_bearing)/100; // reset the old value old_target_bearing = target_bearing; @@ -2396,7 +2395,7 @@ static void update_nav_wp() // create a virtual waypoint that circles the next_WP // Count the degrees we have circulated the WP - //int circle_angle = wrap_360(target_bearing + 3000 + 18000) / 100; + //int16_t circle_angle = wrap_360(target_bearing + 3000 + 18000) / 100; circle_angle += (circle_rate * dTnav); //1° = 0.0174532925 radians @@ -2423,9 +2422,9 @@ static void update_nav_wp() //CIRCLE: angle:29, dist:0, lat:400, lon:242 // debug - //int angleTest = degrees(circle_angle); - //int nroll = nav_roll; - //int npitch = nav_pitch; + //int16_t angleTest = degrees(circle_angle); + //int16_t nroll = nav_roll; + //int16_t npitch = nav_pitch; //Serial.printf("CIRCLE: angle:%d, dist:%d, X:%d, Y:%d, P:%d, R:%d \n", angleTest, (int)wp_distance , (int)long_error, (int)lat_error, npitch, nroll); }else if(wp_control == WP_MODE){ diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 3680b1921a..9135370701 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -514,7 +514,7 @@ static int16_t heli_get_angle_boost(int16_t throttle) { float angle_boost_factor = cos_pitch_x * cos_roll_x; angle_boost_factor = 1.0 - constrain(angle_boost_factor, .5, 1.0); - int throttle_above_mid = max(throttle - motors.throttle_mid,0); + int16_t throttle_above_mid = max(throttle - motors.throttle_mid,0); return throttle + throttle_above_mid*angle_boost_factor; } @@ -525,9 +525,9 @@ static int16_t heli_get_angle_boost(int16_t throttle) #if ACCEL_ALT_HOLD == 2 // z -14.4306 = going up // z -6.4306 = going down -static int get_z_damping() +static int16_t get_z_damping() { - int output; + int16_t output; Z_integrator += get_world_Z_accel() - Z_offset; output = Z_integrator * 3; @@ -546,7 +546,7 @@ float get_world_Z_accel() static void init_z_damper() { Z_offset = 0; - for (int i = 0; i < NUM_G_SAMPLES; i++){ + for (int16_t i = 0; i < NUM_G_SAMPLES; i++){ delay(5); read_AHRS(); Z_offset += get_world_Z_accel(); @@ -593,7 +593,7 @@ float get_world_Z_accel() static void init_z_damper() { estimatedAccelOffset = 0; - for (int i = 0; i < NUM_G_SAMPLES; i++){ + for (int16_t i = 0; i < NUM_G_SAMPLES; i++){ delay(5); read_AHRS(); estimatedAccelOffset += get_world_Z_accel(); @@ -634,7 +634,7 @@ float dead_reckon_Z(float sensedPos, float sensedAccel) return synVelo; } -static int get_z_damping() +static int16_t get_z_damping() { float sensedAccel = get_world_Z_accel(); float sensedPos = current_loc.alt / 100.0; @@ -645,7 +645,7 @@ static int get_z_damping() #else -static int get_z_damping() +static int16_t get_z_damping() { return 0; } diff --git a/ArduCopter/GCS.pde b/ArduCopter/GCS.pde index d8b8d4a3d0..ffec82c7aa 100644 --- a/ArduCopter/GCS.pde +++ b/ArduCopter/GCS.pde @@ -36,12 +36,12 @@ union f_out{ union i_out { byte bytes[2]; - int value; + int16_t value; } intOut; union l_out{ byte bytes[4]; - long value; + int32_t value; } longOut; // Add binary values to the buffer @@ -50,7 +50,7 @@ void write_byte(byte val) mess_buffer[buff_pointer++] = val; } -void write_int(int val ) +void write_int(int16_t val ) { intOut.value = val; mess_buffer[buff_pointer++] = intOut.bytes[0]; @@ -66,7 +66,7 @@ void write_float(float val) mess_buffer[buff_pointer++] = floatOut.bytes[3]; } -void write_long(long val) +void write_long(int32_t val) { longOut.value = val; mess_buffer[buff_pointer++] = longOut.bytes[0]; diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index c644f008e4..0089118281 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -468,7 +468,7 @@ static void NOINLINE send_statustext(mavlink_channel_t chan) // try to send a message, return false if it won't fit in the serial tx buffer static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, uint16_t packet_drops) { - int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; + int16_t payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { // defer any messages on the telemetry port for 1 second after @@ -941,7 +941,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (mavlink_check_target(packet.target_system, packet.target_component)) break; - int freq = 0; // packet frequency + int16_t freq = 0; // packet frequency if (packet.start_stop == 0) freq = 0; // stop sending @@ -1334,7 +1334,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) g.command_total.set_and_save(1); // send acknowledgement 3 times to makes sure it is received - for (int i=0;i<3;i++) + for (int16_t i=0;i<3;i++) mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, type); break; diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 996fef7475..b6ee065a5a 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -62,10 +62,10 @@ MENU2(log_menu, "Log", log_menu_commands, print_log_menu); static bool print_log_menu(void) { - int log_start; - int log_end; - int temp; - int last_log_num = DataFlash.find_last_log(); + int16_t log_start; + int16_t log_end; + int16_t temp; + int16_t last_log_num = DataFlash.find_last_log(); uint16_t num_logs = DataFlash.get_num_logs(); @@ -95,8 +95,8 @@ print_log_menu(void) }else{ Serial.printf_P(PSTR("\n%u logs\n"), (unsigned)num_logs); - for(int i=num_logs;i>=1;i--) { - int last_log_start = log_start, last_log_end = log_end; + for(int16_t i=num_logs;i>=1;i--) { + int16_t last_log_start = log_start, last_log_end = log_end; temp = last_log_num-i+1; DataFlash.get_log_boundaries(temp, log_start, log_end); Serial.printf_P(PSTR("Log %d, start %d, end %d\n"), temp, log_start, log_end); @@ -113,9 +113,9 @@ print_log_menu(void) static int8_t dump_log(uint8_t argc, const Menu::arg *argv) { - int dump_log; - int dump_log_start; - int dump_log_end; + int16_t dump_log; + int16_t dump_log_start; + int16_t dump_log_end; byte last_log_num; // check that the requested log number can be read @@ -316,7 +316,7 @@ static void Log_Read_Raw() /* float logvar; Serial.printf_P(PSTR("RAW,")); - for (int y = 0; y < 9; y++) { + for (int16_t y = 0; y < 9; y++) { logvar = get_float(DataFlash.ReadLong()); Serial.print(logvar); Serial.print(", "); @@ -366,7 +366,7 @@ static void Log_Read_Raw() { float logvar; Serial.printf_P(PSTR("RAW,")); - for (int y = 0; y < 6; y++) { + for (int16_t y = 0; y < 6; y++) { logvar = get_float(DataFlash.ReadLong()); Serial.print(logvar); Serial.print(", "); @@ -731,9 +731,9 @@ static void Log_Read_Cmd() int8_t temp3 = DataFlash.ReadByte(); int8_t temp4 = DataFlash.ReadByte(); int8_t temp5 = DataFlash.ReadByte(); - long temp6 = DataFlash.ReadLong(); - long temp7 = DataFlash.ReadLong(); - long temp8 = DataFlash.ReadLong(); + int32_t temp6 = DataFlash.ReadLong(); + int32_t temp7 = DataFlash.ReadLong(); + int32_t temp8 = DataFlash.ReadLong(); // 1 2 3 4 5 6 7 8 Serial.printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"), @@ -900,9 +900,9 @@ static void Log_Read_PID() } // Read the DataFlash log memory -static void Log_Read(int start_page, int end_page) +static void Log_Read(int16_t start_page, int16_t end_page) { - int packet_count = 0; + int16_t packet_count = 0; #ifdef AIRFRAME_NAME Serial.printf_P(PSTR((AIRFRAME_NAME) @@ -923,12 +923,12 @@ static void Log_Read(int start_page, int end_page) } // Read the DataFlash log memory : Packet Parser -static int Log_Read_Process(int start_page, int end_page) +static int16_t Log_Read_Process(int16_t start_page, int16_t end_page) { byte data; byte log_step = 0; - int page = start_page; - int packet_count = 0; + int16_t page = start_page; + int16_t packet_count = 0; DataFlash.StartRead(start_page); @@ -1031,7 +1031,7 @@ static int Log_Read_Process(int start_page, int end_page) static void Log_Write_Startup() {} static void Log_Read_Startup() {} -static void Log_Read(int start_page, int end_page) {} +static void Log_Read(int16_t start_page, int16_t end_page) {} static void Log_Write_Cmd(byte num, struct Location *wp) {} static void Log_Write_Mode(byte mode) {} static void Log_Write_Raw() {} diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 5fea10ed12..d87781b931 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -385,7 +385,7 @@ static void load_parameters(void) default_dead_zones(); Serial.println_P(PSTR("done.")); } else { - unsigned long before = micros(); + uint32_t before = micros(); // Load all auto-loaded EEPROM variables AP_Param::load_all(); diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 139034b98d..8a200d4944 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -25,7 +25,7 @@ static void read_control_switch() } static byte readSwitch(void){ - int pulsewidth = g.rc_5.radio_in; // default for Arducopter + int16_t pulsewidth = g.rc_5.radio_in; // default for Arducopter if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index a13b857a5b..2cac2eed54 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -9,7 +9,7 @@ // called at 10hz static void arm_motors() { - static int arming_counter; + static int16_t arming_counter; // don't allow arming/disarming in anything but manual if ((g.rc_3.control_in > 0) || (arming_counter > LEVEL_DELAY)){ diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index d6fbc1ed2d..01ed8e0621 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -88,7 +88,7 @@ static void calc_location_error(struct Location *next_loc) #define NAV_ERR_MAX 600 #define NAV_RATE_ERR_MAX 250 -static void calc_loiter(int x_error, int y_error) +static void calc_loiter(int16_t x_error, int16_t y_error) { int32_t p,i,d; // used to capture pid values for logging int32_t output; diff --git a/ArduCopter/planner.pde b/ArduCopter/planner.pde index c8bc1d30b2..a1a9a22ef8 100644 --- a/ArduCopter/planner.pde +++ b/ArduCopter/planner.pde @@ -28,7 +28,7 @@ planner_gcs(uint8_t argc, const Menu::arg *argv) { gcs0.init(&Serial); - int loopcount = 0; + int16_t loopcount = 0; while (1) { if (millis()-fast_loopTimer > 19) { diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index c1dd1993ad..ed49eda0a2 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -116,7 +116,7 @@ setup_show(uint8_t argc, const Menu::arg *argv) static int8_t setup_factory(uint8_t argc, const Menu::arg *argv) { - int c; + int16_t c; Serial.printf_P(PSTR("\n'Y' = factory reset, any other key to abort:\n")); @@ -525,10 +525,10 @@ setup_heli(uint8_t argc, const Menu::arg *argv) { uint8_t active_servo = 0; - int value = 0; - int temp; - int state = 0; // 0 = set rev+pos, 1 = capture min/max - int max_roll=0, max_pitch=0, min_collective=0, max_collective=0, min_tail=0, max_tail=0; + int16_t value = 0; + int16_t temp; + int16_t state = 0; // 0 = set rev+pos, 1 = capture min/max + int16_t max_roll=0, max_pitch=0, min_collective=0, max_collective=0, min_tail=0, max_tail=0; // initialise swash plate motors.init_swash(); @@ -786,8 +786,8 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv) print_hit_enter(); init_compass(); - int _min[3] = {0,0,0}; - int _max[3] = {0,0,0}; + int16_t _min[3] = {0,0,0}; + int16_t _max[3] = {0,0,0}; compass.read(); @@ -973,7 +973,7 @@ static void report_flight_modes() Serial.printf_P(PSTR("Flight modes\n")); print_divider(); - for(int i = 0; i < 6; i++ ){ + for(int16_t i = 0; i < 6; i++ ){ print_switch(i, flight_modes[i], (g.simple_modes & (1< 0){ num--; diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index a00d09281e..8958cf101f 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -256,7 +256,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) #if THROTTLE_FAILSAFE byte fail_test; print_hit_enter(); - for(int i = 0; i < 50; i++){ + for(int16_t i = 0; i < 50; i++){ delay(20); read_radio(); } @@ -409,7 +409,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) delay(50); while(1){ - for(int i = 0; i < 9; i++){ + for(int16_t i = 0; i < 9; i++){ Serial.printf_P(PSTR("%.1f,"),adc.Ch(i)); } Serial.println(); @@ -511,13 +511,13 @@ test_ins(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("g")); - for (int i = 0; i < 3; i++) { + for (int16_t i = 0; i < 3; i++) { Serial.printf_P(PSTR(" %7.4f"), gyro[i]); } Serial.printf_P(PSTR(" a")); - for (int i = 0; i < 3; i++) { + for (int16_t i = 0; i < 3; i++) { Serial.printf_P(PSTR(" %7.4f"),accel[i]); } @@ -624,7 +624,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) //Serial.println(); if(Serial.available() > 0){ Serial.printf_P(PSTR("Y to save\n")); - int c; + int16_t c; c = Serial.read(); do { @@ -823,9 +823,9 @@ test_stab_d(uint8_t argc, const Menu::arg *argv) temp.b.x, temp.b.y, temp.b.z, temp.c.x, temp.c.y, temp.c.z); - int _pitch = degrees(-asin(temp.c.x)); - int _roll = degrees(atan2(temp.c.y, temp.c.z)); - int _yaw = degrees(atan2(temp.b.x, temp.a.x)); + int16_t _pitch = degrees(-asin(temp.c.x)); + int16_t _roll = degrees(atan2(temp.c.y, temp.c.z)); + int16_t _yaw = degrees(atan2(temp.b.x, temp.a.x)); Serial.printf_P(PSTR( "angles\n" "%d \t %d \t %d\n\n"), _pitch,