Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

make a lot more functions and variables static

Browse Source 
this saves about 1k of code space through better compiler optimisation

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2889 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-07-17 10:32:00 +00:00
parent 0b1262a685
commit df6a1b51f3
28 changed files with 291 additions and 291 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopterMega/Attitude.pde
View File

@ -1,7 +1,7 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// XXX TODO: convert these PI rate controlers to a Class // XXX TODO: convert these PI rate controlers to a Class
int static int
get_stabilize_roll(long target_angle) get_stabilize_roll(long target_angle)
{ {
long error; long error;
@ -25,7 +25,7 @@ get_stabilize_roll(long target_angle)
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);
} }
int static int
get_stabilize_pitch(long target_angle) get_stabilize_pitch(long target_angle)
{ {
long error; long error;
@ -49,7 +49,7 @@ get_stabilize_pitch(long target_angle)
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);
} }
int static int
get_stabilize_yaw(long target_angle, float scaler) get_stabilize_yaw(long target_angle, float scaler)
{ {
long error; long error;
@ -74,7 +74,7 @@ get_stabilize_yaw(long target_angle, float scaler)
return (int)constrain(rate, -2500, 2500); return (int)constrain(rate, -2500, 2500);
} }
int static int
get_rate_roll(long target_rate) get_rate_roll(long target_rate)
{ {
long error; long error;
@ -87,7 +87,7 @@ get_rate_roll(long target_rate)
return (int)constrain(target_rate, -2500, 2500); return (int)constrain(target_rate, -2500, 2500);
} }
int static int
get_rate_pitch(long target_rate) get_rate_pitch(long target_rate)
{ {
long error; long error;
@ -100,7 +100,7 @@ get_rate_pitch(long target_rate)
return (int)constrain(target_rate, -2500, 2500); return (int)constrain(target_rate, -2500, 2500);
} }
int static int
get_rate_yaw(long target_rate) get_rate_yaw(long target_rate)
{ {
long error; long error;
@ -115,7 +115,7 @@ get_rate_yaw(long target_rate)
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations // Keeps outdated data out of our calculations
void static void
reset_I(void) reset_I(void)
{ {
// I removed these, they don't seem to be needed. // I removed these, they don't seem to be needed.
@ -128,7 +128,7 @@ throttle control
// user input: // user input:
// ----------- // -----------
int static int
get_throttle(int throttle_input) get_throttle(int throttle_input)
{ {
throttle_input = (float)throttle_input * angle_boost(); throttle_input = (float)throttle_input * angle_boost();
@ -136,7 +136,7 @@ get_throttle(int throttle_input)
return max(throttle_input, 0); return max(throttle_input, 0);
} }
long static long
get_nav_yaw_offset(int yaw_input, int reset) get_nav_yaw_offset(int yaw_input, int reset)
{ {
long _yaw; long _yaw;
@ -158,7 +158,7 @@ get_nav_yaw_offset(int yaw_input, int reset)
} }
} }
/* /*
int alt_hold_velocity() static int alt_hold_velocity()
{ {
// subtract filtered Accel // subtract filtered Accel
float error = abs(next_WP.alt - current_loc.alt); float error = abs(next_WP.alt - current_loc.alt);
@ -167,7 +167,7 @@ int alt_hold_velocity()
return (accels_rot.z + 9.81) * accel_gain * error; return (accels_rot.z + 9.81) * accel_gain * error;
}*/ }*/
float angle_boost() static float angle_boost()
{ {
float temp = cos_pitch_x * cos_roll_x; float temp = cos_pitch_x * cos_roll_x;
temp = 2.0 - constrain(temp, .5, 1.0); temp = 2.0 - constrain(temp, .5, 1.0);

4
ArduCopterMega/Camera.pde
View File

@ -2,7 +2,7 @@
#if CAMERA_STABILIZER == ENABLED #if CAMERA_STABILIZER == ENABLED
void init_camera() static void init_camera()
{ {
g.rc_camera_pitch.set_angle(4500); g.rc_camera_pitch.set_angle(4500);
g.rc_camera_pitch.radio_min = 1000; g.rc_camera_pitch.radio_min = 1000;
@ -15,7 +15,7 @@ void init_camera()
g.rc_camera_roll.radio_max = 2000; g.rc_camera_roll.radio_max = 2000;
} }
void static void
camera_stabilization() camera_stabilization()
{ {
// PITCH // PITCH

28
ArduCopterMega/GCS_Ardupilot.pde
View File

@ -26,17 +26,17 @@ Message Suffix
*/ */
/* /*
void acknowledge(byte id, byte check1, byte check2) {} void static acknowledge(byte id, byte check1, byte check2) {}
void send_message(byte id) {} void static send_message(byte id) {}
void send_message(byte id, long param) {} void static send_message(byte id, long param) {}
void send_message(byte severity, const char *str) {} void static send_message(byte severity, const char *str) {}
*/ */
void acknowledge(byte id, byte check1, byte check2) static void acknowledge(byte id, byte check1, byte check2)
{ {
} }
void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05 static void send_message(byte severity, const char *str) // This is the instance of send_message for message 0x05
{ {
if(severity >= DEBUG_LEVEL){ if(severity >= DEBUG_LEVEL){
SendSer("MSG: "); SendSer("MSG: ");
@ -44,12 +44,12 @@ void send_message(byte severity, const char *str) // This is the instance of se
} }
} }
void send_message(byte id) static void send_message(byte id)
{ {
send_message(id,0l); send_message(id,0l);
} }
void send_message(byte id, long param) static void send_message(byte id, long param)
{ {
switch(id) { switch(id) {
case MSG_ATTITUDE: // ** Attitude message case MSG_ATTITUDE: // ** Attitude message
@ -66,11 +66,11 @@ void send_message(byte id, long param)
} }
} }
void print_current_waypoints() static void print_current_waypoints()
{ {
} }
void print_attitude(void) static void print_attitude(void)
{ {
SendSer("+++"); SendSer("+++");
SendSer("ASP:"); SendSer("ASP:");
@ -84,14 +84,14 @@ void print_attitude(void)
SendSerln(",***"); SendSerln(",***");
} }
void print_control_mode(void) static void print_control_mode(void)
{ {
SendSer("###"); SendSer("###");
SendSer(flight_mode_strings[control_mode]); SendSer(flight_mode_strings[control_mode]);
SendSerln("***"); SendSerln("***");
} }
void print_position(void) static void print_position(void)
{ {
SendSer("!!"); SendSer("!!");
SendSer("!"); SendSer("!");
@ -124,7 +124,7 @@ void print_position(void)
SendSerln(",***"); SendSerln(",***");
} }
void print_waypoint(struct Location *cmd,byte index) static void print_waypoint(struct Location *cmd,byte index)
{ {
SendSer("command #: "); SendSer("command #: ");
SendSer(index, DEC); SendSer(index, DEC);
@ -140,7 +140,7 @@ void print_waypoint(struct Location *cmd,byte index)
SendSerln(cmd->lng,DEC); SendSerln(cmd->lng,DEC);
} }
void print_waypoints() static void print_waypoints()
{ {
} }

20
ArduCopterMega/GCS_IMU_ouput.pde
View File

@ -24,16 +24,16 @@ void send_message(byte id, long param) {}
void send_message(byte severity, const char *str) {} void send_message(byte severity, const char *str) {}
*/ */
void acknowledge(byte id, byte check1, byte check2) static void acknowledge(byte id, byte check1, byte check2)
{ {
} }
void send_message(byte id) static void send_message(byte id)
{ {
send_message(id,0l); send_message(id,0l);
} }
void send_message(byte id, long param) static void send_message(byte id, long param)
{ {
switch(id) { switch(id) {
case MSG_ATTITUDE: case MSG_ATTITUDE:
@ -46,7 +46,7 @@ void send_message(byte id, long param)
} }
} }
void send_message(byte severity, const char *str) static void send_message(byte severity, const char *str)
{ {
if(severity >= DEBUG_LEVEL){ if(severity >= DEBUG_LEVEL){
Serial.print("MSG: "); Serial.print("MSG: ");
@ -54,15 +54,15 @@ void send_message(byte severity, const char *str)
} }
} }
void print_current_waypoints() static void print_current_waypoints()
{ {
} }
void print_control_mode(void) static void print_control_mode(void)
{ {
} }
void print_attitude(void) static void print_attitude(void)
{ {
//Serial.print("!!VER:"); //Serial.print("!!VER:");
//Serial.print(SOFTWARE_VER); //output the software version //Serial.print(SOFTWARE_VER); //output the software version
@ -163,11 +163,11 @@ void print_location(void)
Serial.println("***"); Serial.println("***");
} }
void print_waypoints() static void print_waypoints()
{ {
} }
void print_waypoint(struct Location *cmd,byte index) static void print_waypoint(struct Location *cmd,byte index)
{ {
Serial.print("command #: "); Serial.print("command #: ");
Serial.print(index, DEC); Serial.print(index, DEC);
@ -184,7 +184,7 @@ void print_waypoint(struct Location *cmd,byte index)
} }
long convert_to_dec(float x) static long convert_to_dec(float x)
{ {
return x*10000000; return x*10000000;
} }

28
ArduCopterMega/GCS_Standard.pde
View File

@ -11,7 +11,7 @@
// The functions included in this file are for use with the standard binary communication protocol and standard Ground Control Station // The functions included in this file are for use with the standard binary communication protocol and standard Ground Control Station
void acknowledge(byte id, byte check1, byte check2) { static void acknowledge(byte id, byte check1, byte check2) {
byte mess_buffer[6]; byte mess_buffer[6];
byte mess_ck_a = 0; byte mess_ck_a = 0;
byte mess_ck_b = 0; byte mess_ck_b = 0;
@ -38,11 +38,11 @@ void acknowledge(byte id, byte check1, byte check2) {
SendSer(mess_ck_b); SendSer(mess_ck_b);
} }
void send_message(byte id) { static void send_message(byte id) {
send_message(id, 0l); send_message(id, 0l);
} }
void send_message(byte id, long param) { static void send_message(byte id, long param) {
byte mess_buffer[54]; byte mess_buffer[54];
byte mess_ck_a = 0; byte mess_ck_a = 0;
byte mess_ck_b = 0; byte mess_ck_b = 0;
@ -294,7 +294,7 @@ void send_message(byte id, long param) {
gcs_messages_sent++; gcs_messages_sent++;
} }
void send_message(byte severity, const char *str) // This is the instance of send_message for message MSG_STATUS_TEXT static void send_message(byte severity, const char *str) // This is the instance of send_message for message MSG_STATUS_TEXT
{ {
if(severity >= DEBUG_LEVEL){ if(severity >= DEBUG_LEVEL){
byte length = strlen(str) + 1; byte length = strlen(str) + 1;
@ -328,11 +328,11 @@ void send_message(byte severity, const char *str) // This is the instance of se
} }
} }
void print_current_waypoints() static void print_current_waypoints()
{ {
} }
void print_waypoint(struct Location *cmd, byte index) static void print_waypoint(struct Location *cmd, byte index)
{ {
Serial.print("command #: "); Serial.print("command #: ");
Serial.print(index, DEC); Serial.print(index, DEC);
@ -348,7 +348,7 @@ void print_waypoint(struct Location *cmd, byte index)
Serial.println(cmd->lng, DEC); Serial.println(cmd->lng, DEC);
} }
void print_waypoints() static void print_waypoints()
{ {
} }
@ -357,13 +357,13 @@ void print_waypoints()
#if GCS_PROTOCOL == GCS_PROTOCOL_NONE #if GCS_PROTOCOL == GCS_PROTOCOL_NONE
void acknowledge(byte id, byte check1, byte check2) {} static void acknowledge(byte id, byte check1, byte check2) {}
void send_message(byte id) {} static void send_message(byte id) {}
void send_message(byte id, long param) {} static void send_message(byte id, long param) {}
void send_message(byte severity, const char *str) { static void send_message(byte severity, const char *str) {
Serial.println(str); Serial.println(str);
} }
void print_current_waypoints(){} static void print_current_waypoints(){}
void print_waypoint(struct Location *cmd, byte index){} static void print_waypoint(struct Location *cmd, byte index){}
void print_waypoints(){} static void print_waypoints(){}
#endif #endif

84
ArduCopterMega/Log.pde
View File

@ -194,7 +194,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
return(0); return(0);
} }
int8_t static int8_t
process_logs(uint8_t argc, const Menu::arg *argv) process_logs(uint8_t argc, const Menu::arg *argv)
{ {
log_menu.run(); log_menu.run();
@ -203,7 +203,7 @@ process_logs(uint8_t argc, const Menu::arg *argv)
// finds out how many logs are available // finds out how many logs are available
byte get_num_logs(void) static byte get_num_logs(void)
{ {
int page = 1; int page = 1;
byte data; byte data;
@ -245,7 +245,7 @@ byte get_num_logs(void)
} }
// send the number of the last log? // send the number of the last log?
void start_new_log() static void start_new_log()
{ {
byte num_existing_logs = get_num_logs(); byte num_existing_logs = get_num_logs();
@ -332,7 +332,7 @@ static void get_log_boundaries(byte log_num, int & start_page, int & end_page)
} }
// //
int find_last_log_page(int bottom_page) static int find_last_log_page(int bottom_page)
{ {
int top_page = 4096; int top_page = 4096;
int look_page; int look_page;
@ -354,7 +354,7 @@ int find_last_log_page(int bottom_page)
} }
// Write an GPS packet. Total length : 30 bytes // Write an GPS packet. Total length : 30 bytes
void Log_Write_GPS() static void Log_Write_GPS()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -376,7 +376,7 @@ void Log_Write_GPS()
} }
// Read a GPS packet // Read a GPS packet
void Log_Read_GPS() static void Log_Read_GPS()
{ {
Serial.printf_P(PSTR("GPS, %ld, %d, %d, " Serial.printf_P(PSTR("GPS, %ld, %d, %d, "
"%4.7f, %4.7f, %4.4f, %4.4f, " "%4.7f, %4.7f, %4.4f, %4.4f, "
@ -398,7 +398,7 @@ void Log_Read_GPS()
// Write an raw accel/gyro data packet. Total length : 28 bytes // Write an raw accel/gyro data packet. Total length : 28 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
void Log_Write_Raw() static void Log_Write_Raw()
{ {
Vector3f gyro = imu.get_gyro(); Vector3f gyro = imu.get_gyro();
Vector3f accel = imu.get_accel(); Vector3f accel = imu.get_accel();
@ -430,7 +430,7 @@ void Log_Write_Raw()
#endif #endif
// Read a raw accel/gyro packet // Read a raw accel/gyro packet
void Log_Read_Raw() static void Log_Read_Raw()
{ {
float logvar; float logvar;
Serial.printf_P(PSTR("RAW,")); Serial.printf_P(PSTR("RAW,"));
@ -442,7 +442,7 @@ void Log_Read_Raw()
Serial.println(" "); Serial.println(" ");
} }
void Log_Write_Current() static void Log_Write_Current()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -459,7 +459,7 @@ void Log_Write_Current()
} }
// Read a Current packet // Read a Current packet
void Log_Read_Current() static void Log_Read_Current()
{ {
Serial.printf_P(PSTR("CURR: %d, %ld, %4.4f, %4.4f, %d\n"), Serial.printf_P(PSTR("CURR: %d, %ld, %4.4f, %4.4f, %d\n"),
DataFlash.ReadInt(), DataFlash.ReadInt(),
@ -470,7 +470,7 @@ void Log_Read_Current()
DataFlash.ReadInt()); DataFlash.ReadInt());
} }
void Log_Write_Motors() static void Log_Write_Motors()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -485,7 +485,7 @@ void Log_Write_Motors()
} }
// Read a Current packet // Read a Current packet
void Log_Read_Motors() static void Log_Read_Motors()
{ {
Serial.printf_P(PSTR("MOT: %d, %d, %d, %d\n"), Serial.printf_P(PSTR("MOT: %d, %d, %d, %d\n"),
DataFlash.ReadInt(), DataFlash.ReadInt(),
@ -494,7 +494,7 @@ void Log_Read_Motors()
DataFlash.ReadInt()); DataFlash.ReadInt());
} }
void Log_Write_Nav_Tuning() static void Log_Write_Nav_Tuning()
{ {
Matrix3f tempmat = dcm.get_dcm_matrix(); Matrix3f tempmat = dcm.get_dcm_matrix();
@ -516,7 +516,7 @@ void Log_Write_Nav_Tuning()
} }
void Log_Read_Nav_Tuning() static void Log_Read_Nav_Tuning()
{ {
// 1 2 3 4 // 1 2 3 4
Serial.printf_P(PSTR( "NTUN, %d, %d, %d, %d, " Serial.printf_P(PSTR( "NTUN, %d, %d, %d, %d, "
@ -535,7 +535,7 @@ void Log_Read_Nav_Tuning()
// Write a control tuning packet. Total length : 22 bytes // Write a control tuning packet. Total length : 22 bytes
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
void Log_Write_Control_Tuning() static void Log_Write_Control_Tuning()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -566,7 +566,7 @@ void Log_Write_Control_Tuning()
#endif #endif
// Read an control tuning packet // Read an control tuning packet
void Log_Read_Control_Tuning() static void Log_Read_Control_Tuning()
{ {
Serial.printf_P(PSTR( "CTUN, %d, %d, " Serial.printf_P(PSTR( "CTUN, %d, %d, "
"%d, %d, %d, %d, %1.4f, " "%d, %d, %d, %d, %1.4f, "
@ -594,7 +594,7 @@ void Log_Read_Control_Tuning()
} }
// Write a performance monitoring packet. Total length : 19 bytes // Write a performance monitoring packet. Total length : 19 bytes
void Log_Write_Performance() static void Log_Write_Performance()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -621,7 +621,7 @@ void Log_Write_Performance()
} }
// Read a performance packet // Read a performance packet
void Log_Read_Performance() static void Log_Read_Performance()
{ {
//* //*
long pm_time; long pm_time;
@ -653,7 +653,7 @@ void Log_Read_Performance()
// Write a command processing packet. // Write a command processing packet.
//void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng) //void Log_Write_Cmd(byte num, byte id, byte p1, long alt, long lat, long lng)
void Log_Write_Cmd(byte num, struct Location *wp) static void Log_Write_Cmd(byte num, struct Location *wp)
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -675,7 +675,7 @@ void Log_Write_Cmd(byte num, struct Location *wp)
// Read a command processing packet // Read a command processing packet
void Log_Read_Cmd() static void Log_Read_Cmd()
{ {
Serial.printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"), Serial.printf_P(PSTR( "CMD, %d, %d, %d, %d, %d, %ld, %ld, %ld\n"),
@ -695,7 +695,7 @@ void Log_Read_Cmd()
} }
// Write an attitude packet. Total length : 10 bytes // Write an attitude packet. Total length : 10 bytes
void Log_Write_Attitude() static void Log_Write_Attitude()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -707,7 +707,7 @@ void Log_Write_Attitude()
} }
// Read an attitude packet // Read an attitude packet
void Log_Read_Attitude() static void Log_Read_Attitude()
{ {
Serial.printf_P(PSTR("ATT, %d, %d, %u\n"), Serial.printf_P(PSTR("ATT, %d, %d, %u\n"),
DataFlash.ReadInt(), DataFlash.ReadInt(),
@ -717,7 +717,7 @@ void Log_Read_Attitude()
// Write a mode packet. Total length : 5 bytes // Write a mode packet. Total length : 5 bytes
void Log_Write_Mode(byte mode) static void Log_Write_Mode(byte mode)
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -728,14 +728,14 @@ void Log_Write_Mode(byte mode)
} }
// Read a mode packet // Read a mode packet
void Log_Read_Mode() static void Log_Read_Mode()
{ {
Serial.printf_P(PSTR("MOD:")); Serial.printf_P(PSTR("MOD:"));
Serial.print(flight_mode_strings[DataFlash.ReadByte()]); Serial.print(flight_mode_strings[DataFlash.ReadByte()]);
Serial.printf_P(PSTR(", %d\n"),DataFlash.ReadInt()); Serial.printf_P(PSTR(", %d\n"),DataFlash.ReadInt());
} }
void Log_Write_Startup() static void Log_Write_Startup()
{ {
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -744,14 +744,14 @@ void Log_Write_Startup()
} }
// Read a mode packet // Read a mode packet
void Log_Read_Startup() static void Log_Read_Startup()
{ {
Serial.printf_P(PSTR("START UP\n")); Serial.printf_P(PSTR("START UP\n"));
} }
// Read the DataFlash log memory : Packet Parser // Read the DataFlash log memory : Packet Parser
void Log_Read(int start_page, int end_page) static void Log_Read(int start_page, int end_page)
{ {
byte data; byte data;
byte log_step = 0; byte log_step = 0;
@ -832,20 +832,20 @@ void Log_Read(int start_page, int end_page)
#else // LOGGING_ENABLED #else // LOGGING_ENABLED
void Log_Write_Startup() {} static void Log_Write_Startup() {}
void Log_Read_Startup() {} static void Log_Read_Startup() {}
void Log_Read(int start_page, int end_page) {} static void Log_Read(int start_page, int end_page) {}
void Log_Write_Cmd(byte num, struct Location *wp) {} static void Log_Write_Cmd(byte num, struct Location *wp) {}
void Log_Write_Mode(byte mode) {} static void Log_Write_Mode(byte mode) {}
void start_new_log() {} static void start_new_log() {}
void Log_Write_Raw() {} static void Log_Write_Raw() {}
void Log_Write_GPS() {} static void Log_Write_GPS() {}
void Log_Write_Current() {} static void Log_Write_Current() {}
void Log_Write_Attitude() {} static void Log_Write_Attitude() {}
void Log_Write_Nav_Tuning() {} static void Log_Write_Nav_Tuning() {}
void Log_Write_Control_Tuning() {} static void Log_Write_Control_Tuning() {}
void Log_Write_Motors() {} static void Log_Write_Motors() {}
void Log_Write_Performance() {} static void Log_Write_Performance() {}
int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; } static int8_t process_logs(uint8_t argc, const Menu::arg *argv) { return 0; }
#endif // LOGGING_ENABLED #endif // LOGGING_ENABLED

22
ArduCopterMega/commands.pde
View File

@ -1,6 +1,6 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
void init_commands() static void init_commands()
{ {
// zero is home, but we always load the next command (1), in the code. // zero is home, but we always load the next command (1), in the code.
g.waypoint_index.set_and_save(0); g.waypoint_index.set_and_save(0);
@ -14,7 +14,7 @@ void init_commands()
clear_command_queue(); clear_command_queue();
} }
void init_auto() static void init_auto()
{ {
//if (g.waypoint_index == g.waypoint_total) { //if (g.waypoint_index == g.waypoint_total) {
// Serial.println("ia_f"); // Serial.println("ia_f");
@ -28,13 +28,13 @@ void init_auto()
// forces the loading of a new command // forces the loading of a new command
// queue is emptied after a new command is processed // queue is emptied after a new command is processed
void clear_command_queue(){ static void clear_command_queue(){
next_command.id = NO_COMMAND; next_command.id = NO_COMMAND;
} }
// Getters // Getters
// ------- // -------
struct Location get_command_with_index(int i) static struct Location get_command_with_index(int i)
{ {
struct Location temp; struct Location temp;
@ -91,7 +91,7 @@ struct Location get_command_with_index(int i)
// Setters // Setters
// ------- // -------
void set_command_with_index(struct Location temp, int i) static void set_command_with_index(struct Location temp, int i)
{ {
i = constrain(i, 0, g.waypoint_total.get()); i = constrain(i, 0, g.waypoint_total.get());
uint32_t mem = WP_START_BYTE + (i * WP_SIZE); uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
@ -114,7 +114,7 @@ void set_command_with_index(struct Location temp, int i)
eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7 eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7
} }
void increment_WP_index() static void increment_WP_index()
{ {
if (g.waypoint_index < g.waypoint_total) { if (g.waypoint_index < g.waypoint_total) {
g.waypoint_index.set_and_save(g.waypoint_index + 1); g.waypoint_index.set_and_save(g.waypoint_index + 1);
@ -124,14 +124,14 @@ void increment_WP_index()
SendDebugln(g.waypoint_index,DEC); SendDebugln(g.waypoint_index,DEC);
} }
void decrement_WP_index() static void decrement_WP_index()
{ {
if (g.waypoint_index > 0) { if (g.waypoint_index > 0) {
g.waypoint_index.set_and_save(g.waypoint_index - 1); g.waypoint_index.set_and_save(g.waypoint_index - 1);
} }
} }
long read_alt_to_hold() static long read_alt_to_hold()
{ {
if(g.RTL_altitude < 0) if(g.RTL_altitude < 0)
return current_loc.alt; return current_loc.alt;
@ -145,7 +145,7 @@ long read_alt_to_hold()
// It's not currently used // It's not currently used
//******************************************************************************** //********************************************************************************
Location get_LOITER_home_wp() static Location get_LOITER_home_wp()
{ {
//so we know where we are navigating from //so we know where we are navigating from
next_WP = current_loc; next_WP = current_loc;
@ -162,7 +162,7 @@ This function sets the next waypoint command
It precalculates all the necessary stuff. It precalculates all the necessary stuff.
*/ */
void set_next_WP(struct Location *wp) static void set_next_WP(struct Location *wp)
{ {
//SendDebug("MSG <set_next_wp> wp_index: "); //SendDebug("MSG <set_next_wp> wp_index: ");
//SendDebugln(g.waypoint_index, DEC); //SendDebugln(g.waypoint_index, DEC);
@ -205,7 +205,7 @@ void set_next_WP(struct Location *wp)
// run this at setup on the ground // run this at setup on the ground
// ------------------------------- // -------------------------------
void init_home() static void init_home()
{ {
// block until we get a good fix // block until we get a good fix
// ----------------------------- // -----------------------------

70
ArduCopterMega/commands_logic.pde
View File

@ -3,7 +3,7 @@
/********************************************************************************/ /********************************************************************************/
// Command Event Handlers // Command Event Handlers
/********************************************************************************/ /********************************************************************************/
void handle_process_must() static void handle_process_must()
{ {
// clear nav_lat, this is how we pitch towards the target based on speed // clear nav_lat, this is how we pitch towards the target based on speed
nav_lat = 0; nav_lat = 0;
@ -44,7 +44,7 @@ void handle_process_must()
} }
void handle_process_may() static void handle_process_may()
{ {
switch(next_command.id){ switch(next_command.id){
@ -69,7 +69,7 @@ void handle_process_may()
} }
} }
void handle_process_now() static void handle_process_now()
{ {
switch(next_command.id){ switch(next_command.id){
@ -106,7 +106,7 @@ void handle_process_now()
} }
} }
void handle_no_commands() static void handle_no_commands()
{ {
// we don't want to RTL yet. Maybe this will change in the future. RTL is kinda dangerous. // we don't want to RTL yet. Maybe this will change in the future. RTL is kinda dangerous.
// use landing commands // use landing commands
@ -125,7 +125,7 @@ void handle_no_commands()
// Verify command Handlers // Verify command Handlers
/********************************************************************************/ /********************************************************************************/
bool verify_must() static bool verify_must()
{ {
//Serial.printf("vmust: %d\n", command_must_ID); //Serial.printf("vmust: %d\n", command_must_ID);
@ -166,7 +166,7 @@ bool verify_must()
} }
} }
bool verify_may() static bool verify_may()
{ {
switch(command_may_ID) { switch(command_may_ID) {
@ -197,7 +197,7 @@ bool verify_may()
// //
/********************************************************************************/ /********************************************************************************/
void do_RTL(void) static void do_RTL(void)
{ {
Location temp = home; Location temp = home;
temp.alt = read_alt_to_hold(); temp.alt = read_alt_to_hold();
@ -217,7 +217,7 @@ void do_RTL(void)
// Nav (Must) commands // Nav (Must) commands
/********************************************************************************/ /********************************************************************************/
void do_takeoff() static void do_takeoff()
{ {
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
@ -239,7 +239,7 @@ void do_takeoff()
set_next_WP(&temp); set_next_WP(&temp);
} }
void do_nav_wp() static void do_nav_wp()
{ {
wp_control = WP_MODE; wp_control = WP_MODE;
@ -265,7 +265,7 @@ void do_nav_wp()
} }
} }
void do_land() static void do_land()
{ {
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
@ -287,7 +287,7 @@ void do_land()
set_next_WP(&current_loc); set_next_WP(&current_loc);
} }
void do_loiter_unlimited() static void do_loiter_unlimited()
{ {
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
@ -298,7 +298,7 @@ void do_loiter_unlimited()
set_next_WP(&next_command); set_next_WP(&next_command);
} }
void do_loiter_turns() static void do_loiter_turns()
{ {
/* /*
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
@ -312,7 +312,7 @@ void do_loiter_turns()
*/ */
} }
void do_loiter_time() static void do_loiter_time()
{ {
///* ///*
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
@ -327,7 +327,7 @@ void do_loiter_time()
// Verify Nav (Must) commands // Verify Nav (Must) commands
/********************************************************************************/ /********************************************************************************/
bool verify_takeoff() static bool verify_takeoff()
{ {
// wait until we are ready! // wait until we are ready!
@ -349,7 +349,7 @@ bool verify_takeoff()
} }
} }
bool verify_land() static bool verify_land()
{ {
// land at 1 meter per second // land at 1 meter per second
next_WP.alt = original_alt - ((millis() - land_start) / 20); // condition_value = our initial next_WP.alt = original_alt - ((millis() - land_start) / 20); // condition_value = our initial
@ -376,7 +376,7 @@ bool verify_land()
return false; return false;
} }
bool verify_nav_wp() static bool verify_nav_wp()
{ {
// Altitude checking // Altitude checking
if(next_WP.options & WP_OPTION_ALT_REQUIRED){ if(next_WP.options & WP_OPTION_ALT_REQUIRED){
@ -426,12 +426,12 @@ bool verify_nav_wp()
} }
} }
bool verify_loiter_unlim() static bool verify_loiter_unlim()
{ {
return false; return false;
} }
bool verify_loiter_time() static bool verify_loiter_time()
{ {
//Serial.printf("vlt %ld\n",(millis() - loiter_time)); //Serial.printf("vlt %ld\n",(millis() - loiter_time));
@ -443,7 +443,7 @@ bool verify_loiter_time()
return false; return false;
} }
bool verify_RTL() static bool verify_RTL()
{ {
if (wp_distance <= g.waypoint_radius) { if (wp_distance <= g.waypoint_radius) {
gcs.send_text_P(SEVERITY_LOW,PSTR("Reached home")); gcs.send_text_P(SEVERITY_LOW,PSTR("Reached home"));
@ -457,7 +457,7 @@ bool verify_RTL()
// Condition (May) commands // Condition (May) commands
/********************************************************************************/ /********************************************************************************/
void do_wait_delay() static void do_wait_delay()
{ {
//Serial.print("dwd "); //Serial.print("dwd ");
condition_start = millis(); condition_start = millis();
@ -465,7 +465,7 @@ void do_wait_delay()
Serial.println(condition_value,DEC); Serial.println(condition_value,DEC);
} }
void do_change_alt() static void do_change_alt()
{ {
Location temp = next_WP; Location temp = next_WP;
condition_start = current_loc.alt; condition_start = current_loc.alt;
@ -478,12 +478,12 @@ void do_change_alt()
set_next_WP(&temp); set_next_WP(&temp);
} }
void do_within_distance() static void do_within_distance()
{ {
condition_value = next_command.lat; condition_value = next_command.lat;
} }
void do_yaw() static void do_yaw()
{ {
//Serial.println("dyaw "); //Serial.println("dyaw ");
yaw_tracking = MAV_ROI_NONE; yaw_tracking = MAV_ROI_NONE;
@ -543,7 +543,7 @@ void do_yaw()
// Verify Condition (May) commands // Verify Condition (May) commands
/********************************************************************************/ /********************************************************************************/
bool verify_wait_delay() static bool verify_wait_delay()
{ {
//Serial.print("vwd"); //Serial.print("vwd");
if ((unsigned)(millis() - condition_start) > condition_value){ if ((unsigned)(millis() - condition_start) > condition_value){
@ -555,7 +555,7 @@ bool verify_wait_delay()
return false; return false;
} }
bool verify_change_alt() static bool verify_change_alt()
{ {
if (condition_start < next_WP.alt){ if (condition_start < next_WP.alt){
// we are going higer // we are going higer
@ -573,7 +573,7 @@ bool verify_change_alt()
return false; return false;
} }
bool verify_within_distance() static bool verify_within_distance()
{ {
if (wp_distance < condition_value){ if (wp_distance < condition_value){
condition_value = 0; condition_value = 0;
@ -582,7 +582,7 @@ bool verify_within_distance()
return false; return false;
} }
bool verify_yaw() static bool verify_yaw()
{ {
//Serial.print("vyaw "); //Serial.print("vyaw ");
@ -609,7 +609,7 @@ bool verify_yaw()
// Do (Now) commands // Do (Now) commands
/********************************************************************************/ /********************************************************************************/
void do_target_yaw() static void do_target_yaw()
{ {
yaw_tracking = next_command.p1; yaw_tracking = next_command.p1;
@ -618,12 +618,12 @@ void do_target_yaw()
} }
} }
void do_loiter_at_location() static void do_loiter_at_location()
{ {
next_WP = current_loc; next_WP = current_loc;
} }
void do_jump() static void do_jump()
{ {
struct Location temp; struct Location temp;
if(next_command.lat > 0) { if(next_command.lat > 0) {
@ -638,7 +638,7 @@ void do_jump()
} }
} }
void do_set_home() static void do_set_home()
{ {
if(next_command.p1 == 1) { if(next_command.p1 == 1) {
init_home(); init_home();
@ -651,12 +651,12 @@ void do_set_home()
} }
} }
void do_set_servo() static void do_set_servo()
{ {
APM_RC.OutputCh(next_command.p1 - 1, next_command.alt); APM_RC.OutputCh(next_command.p1 - 1, next_command.alt);
} }
void do_set_relay() static void do_set_relay()
{ {
if (next_command.p1 == 1) { if (next_command.p1 == 1) {
relay_on(); relay_on();
@ -667,7 +667,7 @@ void do_set_relay()
} }
} }
void do_repeat_servo() static void do_repeat_servo()
{ {
event_id = next_command.p1 - 1; event_id = next_command.p1 - 1;
@ -696,7 +696,7 @@ void do_repeat_servo()
} }
} }
void do_repeat_relay() static void do_repeat_relay()
{ {
event_id = RELAY_TOGGLE; event_id = RELAY_TOGGLE;
event_timer = 0; event_timer = 0;

14
ArduCopterMega/commands_process.pde
View File

@ -2,7 +2,7 @@
// For changing active command mid-mission // For changing active command mid-mission
//---------------------------------------- //----------------------------------------
void change_command(uint8_t index) static void change_command(uint8_t index)
{ {
struct Location temp = get_command_with_index(index); struct Location temp = get_command_with_index(index);
@ -18,7 +18,7 @@ void change_command(uint8_t index)
// called by 10 Hz Medium loop // called by 10 Hz Medium loop
// --------------------------- // ---------------------------
void update_commands(void) static void update_commands(void)
{ {
// fill command queue with a new command if available // fill command queue with a new command if available
if(next_command.id == NO_COMMAND){ if(next_command.id == NO_COMMAND){
@ -62,7 +62,7 @@ void update_commands(void)
} }
// called with GPS navigation update - not constantly // called with GPS navigation update - not constantly
void verify_commands(void) static void verify_commands(void)
{ {
if(verify_must()){ if(verify_must()){
Serial.printf("verified must cmd %d\n" , command_must_index); Serial.printf("verified must cmd %d\n" , command_must_index);
@ -81,7 +81,7 @@ void verify_commands(void)
} }
} }
bool static bool
process_next_command() process_next_command()
{ {
// these are Navigation/Must commands // these are Navigation/Must commands
@ -142,7 +142,7 @@ process_next_command()
/**************************************************/ /**************************************************/
// These functions implement the commands. // These functions implement the commands.
/**************************************************/ /**************************************************/
void process_must() static void process_must()
{ {
//gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>")); //gcs.send_text_P(SEVERITY_LOW,PSTR("New cmd: <process_must>"));
//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); //gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
@ -161,7 +161,7 @@ void process_must()
} }
void process_may() static void process_may()
{ {
//gcs.send_text_P(SEVERITY_LOW,PSTR("<process_may>")); //gcs.send_text_P(SEVERITY_LOW,PSTR("<process_may>"));
//gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); //gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
@ -171,7 +171,7 @@ void process_may()
handle_process_may(); handle_process_may();
} }
void process_now() static void process_now()
{ {
Serial.print("pnow"); Serial.print("pnow");
handle_process_now(); handle_process_now();

18
ArduCopterMega/control_modes.pde
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
void read_control_switch() static void read_control_switch()
{ {
byte switchPosition = readSwitch(); byte switchPosition = readSwitch();
//motor_armed = (switchPosition < 5); //motor_armed = (switchPosition < 5);
@ -18,7 +18,7 @@ void read_control_switch()
} }
} }
byte readSwitch(void){ static byte readSwitch(void){
int pulsewidth = g.rc_5.radio_in; // default for Arducopter int pulsewidth = g.rc_5.radio_in; // default for Arducopter
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
@ -29,7 +29,7 @@ byte readSwitch(void){
return 0; return 0;
} }
void reset_control_switch() static void reset_control_switch()
{ {
oldSwitchPosition = -1; oldSwitchPosition = -1;
read_control_switch(); read_control_switch();
@ -37,17 +37,17 @@ void reset_control_switch()
//SendDebugln(oldSwitchPosition , DEC); //SendDebugln(oldSwitchPosition , DEC);
} }
void update_servo_switches() static void update_servo_switches()
{ {
} }
boolean trim_flag; static boolean trim_flag;
unsigned long trim_timer; static unsigned long trim_timer;
// read at 10 hz // read at 10 hz
// set this to your trainer switch // set this to your trainer switch
void read_trim_switch() static void read_trim_switch()
{ {
// switch is engaged // switch is engaged
if (g.rc_7.control_in > 800){ if (g.rc_7.control_in > 800){
@ -67,7 +67,7 @@ void read_trim_switch()
} }
} }
void auto_trim() static void auto_trim()
{ {
if(auto_level_counter > 0){ if(auto_level_counter > 0){
g.rc_1.dead_zone = 60; // 60 = .6 degrees g.rc_1.dead_zone = 60; // 60 = .6 degrees
@ -91,7 +91,7 @@ void auto_trim()
void trim_accel() static void trim_accel()
{ {
g.pid_stabilize_roll.reset_I(); g.pid_stabilize_roll.reset_I();
g.pid_stabilize_pitch.reset_I(); g.pid_stabilize_pitch.reset_I();

14
ArduCopterMega/events.pde
View File

@ -4,7 +4,7 @@
This event will be called when the failsafe changes This event will be called when the failsafe changes
boolean failsafe reflects the current state boolean failsafe reflects the current state
*/ */
void failsafe_on_event() static void failsafe_on_event()
{ {
// This is how to handle a failsafe. // This is how to handle a failsafe.
switch(control_mode) switch(control_mode)
@ -22,7 +22,7 @@ void failsafe_on_event()
} }
} }
void failsafe_off_event() static void failsafe_off_event()
{ {
if (g.throttle_fs_action == 2){ if (g.throttle_fs_action == 2){
// We're back in radio contact // We're back in radio contact
@ -46,14 +46,14 @@ void failsafe_off_event()
} }
} }
void low_battery_event(void) static void low_battery_event(void)
{ {
gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
set_mode(RTL); set_mode(RTL);
} }
void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY static void update_events() // 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) < event_delay)
return; return;
@ -79,17 +79,17 @@ void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_R
} }
} }
void relay_on() static void relay_on()
{ {
PORTL |= B00000100; PORTL |= B00000100;
} }
void relay_off() static void relay_off()
{ {
PORTL &= ~B00000100; PORTL &= ~B00000100;
} }
void relay_toggle() static void relay_toggle()
{ {
PORTL ^= B00000100; PORTL ^= B00000100;
} }

18
ArduCopterMega/heli.pde
View File

@ -2,10 +2,10 @@
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
int heli_manual_override = false; static int heli_manual_override = false;
float rollPitch_impact_on_collective = 0; static float rollPitch_impact_on_collective = 0;
void heli_init_swash() static void heli_init_swash()
{ {
int i; int i;
int tilt_max[CH_3+1]; int tilt_max[CH_3+1];
@ -50,7 +50,7 @@ void heli_init_swash()
g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3]; g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3];
} }
void heli_move_servos_to_mid() static void heli_move_servos_to_mid()
{ {
heli_move_swash(0,0,1500,0); heli_move_swash(0,0,1500,0);
} }
@ -63,7 +63,7 @@ void heli_move_servos_to_mid()
// collective: 1000 ~ 2000 // collective: 1000 ~ 2000
// yaw: -4500 ~ 4500?? // yaw: -4500 ~ 4500??
// //
void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out)
{ {
// ensure values are acceptable: // ensure values are acceptable:
roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max); roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max);
@ -117,7 +117,7 @@ void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out)
} }
// these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better // these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better
void output_motors_armed() static void output_motors_armed()
{ {
//static int counter = 0; //static int counter = 0;
g.rc_1.calc_pwm(); g.rc_1.calc_pwm();
@ -157,14 +157,14 @@ void output_motors_armed()
} }
// for helis - armed or disarmed we allow servos to move // for helis - armed or disarmed we allow servos to move
void output_motors_disarmed() static void output_motors_disarmed()
{ {
//heli_move_servos_to_mid(); //heli_move_servos_to_mid();
output_motors_armed(); output_motors_armed();
} }
void output_motor_test() static void output_motor_test()
{ {
} }
#endif // HELI_FRAME #endif // HELI_FRAME

12
ArduCopterMega/leds.pde
View File

@ -1,4 +1,4 @@
void update_lights() static void update_lights()
{ {
switch(led_mode){ switch(led_mode){
case NORMAL_LEDS: case NORMAL_LEDS:
@ -12,7 +12,7 @@ void update_lights()
} }
} }
void update_GPS_light(void) static void update_GPS_light(void)
{ {
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data // GPS LED on if we have a fix or Blink GPS LED if we are receiving data
// --------------------------------------------------------------------- // ---------------------------------------------------------------------
@ -40,7 +40,7 @@ void update_GPS_light(void)
} }
} }
void update_motor_light(void) static void update_motor_light(void)
{ {
if(motor_armed == false){ if(motor_armed == false){
motor_light = !motor_light; motor_light = !motor_light;
@ -59,7 +59,7 @@ void update_motor_light(void)
} }
} }
void dancing_light() static void dancing_light()
{ {
static byte step; static byte step;
@ -84,7 +84,7 @@ void dancing_light()
break; break;
} }
} }
void clear_leds() static void clear_leds()
{ {
digitalWrite(A_LED_PIN, LOW); digitalWrite(A_LED_PIN, LOW);
digitalWrite(B_LED_PIN, LOW); digitalWrite(B_LED_PIN, LOW);
@ -92,7 +92,7 @@ void clear_leds()
} }
#if MOTOR_LEDS == 1 #if MOTOR_LEDS == 1
void update_motor_leds(void) static void update_motor_leds(void)
{ {
// blink rear // blink rear
static bool blink; static bool blink;

6
ArduCopterMega/motors.pde
View File

@ -7,7 +7,7 @@
// called at 10hz // called at 10hz
void arm_motors() static void arm_motors()
{ {
static int arming_counter; static int arming_counter;
@ -77,7 +77,7 @@ void arm_motors()
/***************************************** /*****************************************
* Set the flight control servos based on the current calculated values * Set the flight control servos based on the current calculated values
*****************************************/ *****************************************/
void static void
set_servos_4() set_servos_4()
{ {
if (motor_armed == true && motor_auto_armed == true) { if (motor_armed == true && motor_auto_armed == true) {
@ -175,4 +175,4 @@ set_servos_4()
nav_throttle, nav_throttle,
angle_boost()); angle_boost());
*/ */
//} //}

8
ArduCopterMega/motors_hexa.pde
View File

@ -2,7 +2,7 @@
#if FRAME_CONFIG == HEXA_FRAME #if FRAME_CONFIG == HEXA_FRAME
void output_motors_armed() static void output_motors_armed()
{ {
int roll_out, pitch_out; int roll_out, pitch_out;
int out_min = g.rc_3.radio_min; int out_min = g.rc_3.radio_min;
@ -112,7 +112,7 @@ void output_motors_armed()
#endif #endif
} }
void output_motors_disarmed() static void output_motors_disarmed()
{ {
if(g.rc_3.control_in > 0){ if(g.rc_3.control_in > 0){
// we have pushed up the throttle // we have pushed up the throttle
@ -134,7 +134,7 @@ void output_motors_disarmed()
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
} }
void output_motor_test() static void output_motor_test()
{ {
motor_out[CH_1] = g.rc_3.radio_min; motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[CH_2] = g.rc_3.radio_min;
@ -224,4 +224,4 @@ void output_motor_test()
} }
*/ */
#endif #endif

6
ArduCopterMega/motors_octa.pde
View File

@ -2,7 +2,7 @@
#if FRAME_CONFIG == OCTA_FRAME #if FRAME_CONFIG == OCTA_FRAME
void output_motors_armed() static void output_motors_armed()
{ {
int roll_out, pitch_out; int roll_out, pitch_out;
int out_min = g.rc_3.radio_min; int out_min = g.rc_3.radio_min;
@ -166,7 +166,7 @@ void output_motors_armed()
#endif #endif
} }
void output_motors_disarmed() static void output_motors_disarmed()
{ {
if(g.rc_3.control_in > 0){ if(g.rc_3.control_in > 0){
// we have pushed up the throttle // we have pushed up the throttle
@ -190,7 +190,7 @@ void output_motors_disarmed()
APM_RC.OutputCh(CH_11, g.rc_3.radio_min); APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
} }
void output_motor_test() static void output_motor_test()
{ {
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50); APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50);

6
ArduCopterMega/motors_octa_quad.pde
View File

@ -2,7 +2,7 @@
#if FRAME_CONFIG == OCTA_QUAD_FRAME #if FRAME_CONFIG == OCTA_QUAD_FRAME
void output_motors_armed() static void output_motors_armed()
{ {
int roll_out, pitch_out; int roll_out, pitch_out;
int out_min = g.rc_3.radio_min; int out_min = g.rc_3.radio_min;
@ -137,7 +137,7 @@ void output_motors_armed()
#endif #endif
} }
void output_motors_disarmed() static void output_motors_disarmed()
{ {
if(g.rc_3.control_in > 0){ if(g.rc_3.control_in > 0){
// we have pushed up the throttle // we have pushed up the throttle
@ -161,7 +161,7 @@ void output_motors_disarmed()
APM_RC.OutputCh(CH_11, g.rc_3.radio_min); APM_RC.OutputCh(CH_11, g.rc_3.radio_min);
} }
void output_motor_test() static void output_motor_test()
{ {
} }

8
ArduCopterMega/motors_quad.pde
View File

@ -2,7 +2,7 @@
#if FRAME_CONFIG == QUAD_FRAME #if FRAME_CONFIG == QUAD_FRAME
void output_motors_armed() static void output_motors_armed()
{ {
int roll_out, pitch_out; int roll_out, pitch_out;
int out_min = g.rc_3.radio_min; int out_min = g.rc_3.radio_min;
@ -87,7 +87,7 @@ void output_motors_armed()
APM_RC.Force_Out2_Out3(); APM_RC.Force_Out2_Out3();
} }
void output_motors_disarmed() static void output_motors_disarmed()
{ {
if(g.rc_3.control_in > 0){ if(g.rc_3.control_in > 0){
// we have pushed up the throttle // we have pushed up the throttle
@ -111,7 +111,7 @@ void output_motors_disarmed()
APM_RC.Force_Out2_Out3(); APM_RC.Force_Out2_Out3();
} }
void output_motor_test() static void output_motor_test()
{ {
motor_out[CH_1] = g.rc_3.radio_min; motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[CH_2] = g.rc_3.radio_min;
@ -165,4 +165,4 @@ void output_motor_test()
APM_RC.OutputCh(CH_4, motor_out[CH_4]); APM_RC.OutputCh(CH_4, motor_out[CH_4]);
} }
#endif #endif

8
ArduCopterMega/motors_tri.pde
View File

@ -1,7 +1,7 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == TRI_FRAME #if FRAME_CONFIG == TRI_FRAME
void output_motors_armed() static void output_motors_armed()
{ {
int out_min = g.rc_3.radio_min; int out_min = g.rc_3.radio_min;
int out_max = g.rc_3.radio_max; int out_max = g.rc_3.radio_max;
@ -64,7 +64,7 @@ void output_motors_armed()
#endif #endif
} }
void output_motors_disarmed() static void output_motors_disarmed()
{ {
if(g.rc_3.control_in > 0){ if(g.rc_3.control_in > 0){
// we have pushed up the throttle // we have pushed up the throttle
@ -83,7 +83,7 @@ void output_motors_disarmed()
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
} }
void output_motor_test() static void output_motor_test()
{ {
motor_out[CH_1] = g.rc_3.radio_min; motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[CH_2] = g.rc_3.radio_min;
@ -107,4 +107,4 @@ void output_motor_test()
APM_RC.OutputCh(CH_4, motor_out[CH_4]); APM_RC.OutputCh(CH_4, motor_out[CH_4]);
} }
#endif #endif

8
ArduCopterMega/motors_y6.pde
View File

@ -2,7 +2,7 @@
#if FRAME_CONFIG == Y6_FRAME #if FRAME_CONFIG == Y6_FRAME
void output_motors_armed() static void output_motors_armed()
{ {
int out_min = g.rc_3.radio_min; int out_min = g.rc_3.radio_min;
int out_max = g.rc_3.radio_max; int out_max = g.rc_3.radio_max;
@ -94,7 +94,7 @@ void output_motors_armed()
#endif #endif
} }
void output_motors_disarmed() static void output_motors_disarmed()
{ {
if(g.rc_3.control_in > 0){ if(g.rc_3.control_in > 0){
// we have pushed up the throttle // we have pushed up the throttle
@ -116,7 +116,7 @@ void output_motors_disarmed()
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
} }
void output_motor_test() static void output_motor_test()
{ {
motor_out[CH_1] = g.rc_3.radio_min; motor_out[CH_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[CH_2] = g.rc_3.radio_min;
@ -149,4 +149,4 @@ void output_motor_test()
APM_RC.OutputCh(CH_8, motor_out[CH_8]); APM_RC.OutputCh(CH_8, motor_out[CH_8]);
} }
#endif #endif

40
ArduCopterMega/navigation.pde
View File

@ -3,7 +3,7 @@
//**************************************************************** //****************************************************************
// Function that will calculate the desired direction to fly and distance // Function that will calculate the desired direction to fly and distance
//**************************************************************** //****************************************************************
void navigate() static void navigate()
{ {
// do not navigate with corrupt data // do not navigate with corrupt data
// --------------------------------- // ---------------------------------
@ -36,14 +36,14 @@ void navigate()
nav_bearing = target_bearing; nav_bearing = target_bearing;
} }
bool check_missed_wp() static bool check_missed_wp()
{ {
long temp = target_bearing - saved_target_bearing; long temp = target_bearing - saved_target_bearing;
temp = wrap_180(temp); temp = wrap_180(temp);
return (abs(temp) > 10000); //we pased the waypoint by 10 ° return (abs(temp) > 10000); //we pased the waypoint by 10 °
} }
int static int
get_nav_throttle(long error) get_nav_throttle(long error)
{ {
int throttle; int throttle;
@ -62,7 +62,7 @@ get_nav_throttle(long error)
} }
#define DIST_ERROR_MAX 1800 #define DIST_ERROR_MAX 1800
void calc_loiter_nav() static void calc_loiter_nav()
{ {
/* /*
Becuase we are using lat and lon to do our distance errors here's a quick chart: Becuase we are using lat and lon to do our distance errors here's a quick chart:
@ -88,7 +88,7 @@ void calc_loiter_nav()
nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav, 1.0); // Y invert lat (for pitch) nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav, 1.0); // Y invert lat (for pitch)
} }
void calc_loiter_output() static void calc_loiter_output()
{ {
// rotate the vector // rotate the vector
nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * -cos_yaw_x; nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * -cos_yaw_x;
@ -118,7 +118,7 @@ void calc_loiter_output()
//SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward //SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward
} }
void calc_simple_nav() static void calc_simple_nav()
{ {
// no dampening here in SIMPLE mode // no dampening here in SIMPLE mode
nav_lat = constrain((wp_distance * 100), -4500, 4500); // +- 20m max error nav_lat = constrain((wp_distance * 100), -4500, 4500); // +- 20m max error
@ -126,7 +126,7 @@ void calc_simple_nav()
//nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° //nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36°
} }
void calc_nav_output() static void calc_nav_output()
{ {
// get the sin and cos of the bearing error - rotated 90° // get the sin and cos of the bearing error - rotated 90°
float sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100)); float sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
@ -138,7 +138,7 @@ void calc_nav_output()
} }
// called after we get GPS read // called after we get GPS read
void calc_rate_nav() static void calc_rate_nav()
{ {
// which direction are we moving? // which direction are we moving?
long target_error = target_bearing - g_gps->ground_course; long target_error = target_bearing - g_gps->ground_course;
@ -167,18 +167,18 @@ void calc_rate_nav()
nav_lat = constrain(nav_lat, -4000, 4000); // +- max error nav_lat = constrain(nav_lat, -4000, 4000); // +- max error
} }
void calc_bearing_error() static void calc_bearing_error()
{ {
bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = nav_bearing - dcm.yaw_sensor;
bearing_error = wrap_180(bearing_error); bearing_error = wrap_180(bearing_error);
} }
void calc_altitude_error() static void calc_altitude_error()
{ {
altitude_error = next_WP.alt - current_loc.alt; altitude_error = next_WP.alt - current_loc.alt;
} }
void calc_altitude_smoothing_error() static void calc_altitude_smoothing_error()
{ {
// limit climb rates - we draw a straight line between first location and edge of waypoint_radius // limit climb rates - we draw a straight line between first location and edge of waypoint_radius
target_altitude = next_WP.alt - ((wp_distance * (next_WP.alt - prev_WP.alt)) / (wp_totalDistance - g.waypoint_radius)); target_altitude = next_WP.alt - ((wp_distance * (next_WP.alt - prev_WP.alt)) / (wp_totalDistance - g.waypoint_radius));
@ -193,21 +193,21 @@ void calc_altitude_smoothing_error()
altitude_error = target_altitude - current_loc.alt; altitude_error = target_altitude - current_loc.alt;
} }
long wrap_360(long error) static long wrap_360(long error)
{ {
if (error > 36000) error -= 36000; if (error > 36000) error -= 36000;
if (error < 0) error += 36000; if (error < 0) error += 36000;
return error; return error;
} }
long wrap_180(long error) static long wrap_180(long error)
{ {
if (error > 18000) error -= 36000; if (error > 18000) error -= 36000;
if (error < -18000) error += 36000; if (error < -18000) error += 36000;
return error; return error;
} }
void update_crosstrack(void) static void update_crosstrack(void)
{ {
// Crosstrack Error // Crosstrack Error
// ---------------- // ----------------
@ -219,19 +219,19 @@ void update_crosstrack(void)
} }
} }
long cross_track_test() static long cross_track_test()
{ {
long temp = target_bearing - crosstrack_bearing; long temp = target_bearing - crosstrack_bearing;
temp = wrap_180(temp); temp = wrap_180(temp);
return abs(temp); return abs(temp);
} }
void reset_crosstrack() static void reset_crosstrack()
{ {
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
} }
long get_altitude_above_home(void) static long get_altitude_above_home(void)
{ {
// This is the altitude above the home location // This is the altitude above the home location
// The GPS gives us altitude at Sea Level // The GPS gives us altitude at Sea Level
@ -241,7 +241,7 @@ long get_altitude_above_home(void)
} }
// distance is returned in meters // distance is returned in meters
long get_distance(struct Location *loc1, struct Location *loc2) static long get_distance(struct Location *loc1, struct Location *loc2)
{ {
//if(loc1->lat == 0 || loc1->lng == 0) //if(loc1->lat == 0 || loc1->lng == 0)
// return -1; // return -1;
@ -252,12 +252,12 @@ long get_distance(struct Location *loc1, struct Location *loc2)
return sqrt(sq(dlat) + sq(dlong)) * .01113195; return sqrt(sq(dlat) + sq(dlong)) * .01113195;
} }
long get_alt_distance(struct Location *loc1, struct Location *loc2) static long get_alt_distance(struct Location *loc1, struct Location *loc2)
{ {
return abs(loc1->alt - loc2->alt); return abs(loc1->alt - loc2->alt);
} }
long get_bearing(struct Location *loc1, struct Location *loc2) static long get_bearing(struct Location *loc1, struct Location *loc2)
{ {
long off_x = loc2->lng - loc1->lng; long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp; long off_y = (loc2->lat - loc1->lat) * scaleLongUp;

2
ArduCopterMega/planner.pde
View File

@ -15,7 +15,7 @@ const struct Menu::command planner_menu_commands[] PROGMEM = {
// A Macro to create the Menu // A Macro to create the Menu
MENU(planner_menu, "planner", planner_menu_commands); MENU(planner_menu, "planner", planner_menu_commands);
int8_t static int8_t
planner_mode(uint8_t argc, const Menu::arg *argv) planner_mode(uint8_t argc, const Menu::arg *argv)
{ {
Serial.printf_P(PSTR("Planner Mode\nNot intended for manual use\n\n")); Serial.printf_P(PSTR("Planner Mode\nNot intended for manual use\n\n"));

14
ArduCopterMega/radio.pde
View File

@ -2,10 +2,10 @@
//Function that will read the radio data, limit servos and trigger a failsafe //Function that will read the radio data, limit servos and trigger a failsafe
// ---------------------------------------------------------------------------- // ----------------------------------------------------------------------------
byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
void init_rc_in() static void init_rc_in()
{ {
// set rc channel ranges // set rc channel ranges
g.rc_1.set_angle(4500); g.rc_1.set_angle(4500);
@ -58,7 +58,7 @@ void init_rc_in()
#endif #endif
} }
void init_rc_out() static void init_rc_out()
{ {
#if ARM_AT_STARTUP == 1 #if ARM_AT_STARTUP == 1
motor_armed = 1; motor_armed = 1;
@ -115,7 +115,7 @@ void output_min()
#endif #endif
} }
void read_radio() static void read_radio()
{ {
if (APM_RC.GetState() == 1){ if (APM_RC.GetState() == 1){
@ -145,7 +145,7 @@ void read_radio()
} }
} }
void throttle_failsafe(uint16_t pwm) static void throttle_failsafe(uint16_t pwm)
{ {
if(g.throttle_fs_enabled == 0) if(g.throttle_fs_enabled == 0)
return; return;
@ -187,7 +187,7 @@ void throttle_failsafe(uint16_t pwm)
} }
} }
void trim_radio() static void trim_radio()
{ {
for (byte i = 0; i < 30; i++){ for (byte i = 0; i < 30; i++){
read_radio(); read_radio();
@ -202,7 +202,7 @@ void trim_radio()
g.rc_4.save_eeprom(); g.rc_4.save_eeprom();
} }
void trim_yaw() static void trim_yaw()
{ {
for (byte i = 0; i < 30; i++){ for (byte i = 0; i < 30; i++){
read_radio(); read_radio();

6
ArduCopterMega/read_commands.pde
View File

@ -4,7 +4,7 @@
#define INPUT_BUF_LEN 40 #define INPUT_BUF_LEN 40
char input_buffer[INPUT_BUF_LEN]; char input_buffer[INPUT_BUF_LEN];
void readCommands(void) static void readCommands(void)
{ {
static byte header[2]; static byte header[2];
const byte read_GS_header[] = {0x21, 0x21}; //!! Used to verify the payload msg header const byte read_GS_header[] = {0x21, 0x21}; //!! Used to verify the payload msg header
@ -56,7 +56,7 @@ void readCommands(void)
// Commands can be sent as !!a:100|b:200|c:1 // Commands can be sent as !!a:100|b:200|c:1
// ----------------------------------------- // -----------------------------------------
void parseCommand(char *buffer) static void parseCommand(char *buffer)
{ {
Serial.println("got cmd "); Serial.println("got cmd ");
Serial.println(buffer); Serial.println(buffer);
@ -109,4 +109,4 @@ void parseCommand(char *buffer)
} }
} }
#endif #endif

14
ArduCopterMega/sensors.pde
View File

@ -3,9 +3,9 @@
// Sensors are not available in HIL_MODE_ATTITUDE // Sensors are not available in HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
void ReadSCP1000(void) {} static void ReadSCP1000(void) {}
void init_barometer(void) static void init_barometer(void)
{ {
#if HIL_MODE == HIL_MODE_SENSORS #if HIL_MODE == HIL_MODE_SENSORS
hil.update(); // look for inbound hil packets for initialization hil.update(); // look for inbound hil packets for initialization
@ -32,7 +32,7 @@ void init_barometer(void)
//SendDebugln("barometer calibration complete."); //SendDebugln("barometer calibration complete.");
} }
long read_baro_filtered(void) static long read_baro_filtered(void)
{ {
long pressure = 0; long pressure = 0;
@ -58,7 +58,7 @@ long read_baro_filtered(void)
return pressure /= BARO_FILTER_SIZE; return pressure /= BARO_FILTER_SIZE;
} }
long read_barometer(void) static long read_barometer(void)
{ {
float x, scaling, temp; float x, scaling, temp;
@ -73,19 +73,19 @@ long read_barometer(void)
} }
// in M/S * 100 // in M/S * 100
void read_airspeed(void) static void read_airspeed(void)
{ {
} }
void zero_airspeed(void) static void zero_airspeed(void)
{ {
} }
#endif // HIL_MODE != HIL_MODE_ATTITUDE #endif // HIL_MODE != HIL_MODE_ATTITUDE
void read_battery(void) static void read_battery(void)
{ {
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9; battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9; battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;

64
ArduCopterMega/setup.pde
View File

@ -49,7 +49,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
MENU(setup_menu, "setup", setup_menu_commands); MENU(setup_menu, "setup", setup_menu_commands);
// Called from the top-level menu to run the setup menu. // Called from the top-level menu to run the setup menu.
int8_t static int8_t
setup_mode(uint8_t argc, const Menu::arg *argv) setup_mode(uint8_t argc, const Menu::arg *argv)
{ {
// Give the user some guidance // Give the user some guidance
@ -233,7 +233,7 @@ setup_esc(uint8_t argc, const Menu::arg *argv)
} }
} }
void static void
init_esc() init_esc()
{ {
g.esc_calibrate.set_and_save(0); g.esc_calibrate.set_and_save(0);
@ -654,7 +654,7 @@ setup_gyro(uint8_t argc, const Menu::arg *argv)
#endif // FRAME_CONFIG == HELI #endif // FRAME_CONFIG == HELI
void clear_offsets() static void clear_offsets()
{ {
Vector3f _offsets(0.0,0.0,0.0); Vector3f _offsets(0.0,0.0,0.0);
compass.set_offsets(_offsets); compass.set_offsets(_offsets);
@ -730,7 +730,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
// CLI reports // CLI reports
/***************************************************************************/ /***************************************************************************/
void report_batt_monitor() static void report_batt_monitor()
{ {
Serial.printf_P(PSTR("\nBatt Mointor\n")); Serial.printf_P(PSTR("\nBatt Mointor\n"));
print_divider(); print_divider();
@ -742,7 +742,7 @@ void report_batt_monitor()
print_blanks(2); print_blanks(2);
} }
void report_wp(byte index = 255) static void report_wp(byte index = 255)
{ {
if(index == 255){ if(index == 255){
for(byte i = 0; i <= g.waypoint_total; i++){ for(byte i = 0; i <= g.waypoint_total; i++){
@ -755,7 +755,7 @@ void report_wp(byte index = 255)
} }
} }
void print_wp(struct Location *cmd, byte index) static void print_wp(struct Location *cmd, byte index)
{ {
Serial.printf_P(PSTR("command #: %d id:%d op:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), Serial.printf_P(PSTR("command #: %d id:%d op:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)index, (int)index,
@ -767,7 +767,7 @@ void print_wp(struct Location *cmd, byte index)
cmd->lng); cmd->lng);
} }
void report_gps() static void report_gps()
{ {
Serial.printf_P(PSTR("\nGPS\n")); Serial.printf_P(PSTR("\nGPS\n"));
print_divider(); print_divider();
@ -775,7 +775,7 @@ void report_gps()
print_blanks(2); print_blanks(2);
} }
void report_sonar() static void report_sonar()
{ {
g.sonar_enabled.load(); g.sonar_enabled.load();
Serial.printf_P(PSTR("Sonar\n")); Serial.printf_P(PSTR("Sonar\n"));
@ -784,14 +784,14 @@ void report_sonar()
print_blanks(2); print_blanks(2);
} }
void report_version() static void report_version()
{ {
Serial.printf_P(PSTR("FW Version %d\n"),(int)g.format_version.get()); Serial.printf_P(PSTR("FW Version %d\n"),(int)g.format_version.get());
print_divider(); print_divider();
print_blanks(2); print_blanks(2);
} }
void report_frame() static void report_frame()
{ {
Serial.printf_P(PSTR("Frame\n")); Serial.printf_P(PSTR("Frame\n"));
print_divider(); print_divider();
@ -822,7 +822,7 @@ void report_frame()
print_blanks(2); print_blanks(2);
} }
void report_radio() static void report_radio()
{ {
Serial.printf_P(PSTR("Radio\n")); Serial.printf_P(PSTR("Radio\n"));
print_divider(); print_divider();
@ -832,7 +832,7 @@ void report_radio()
} }
/* /*
void report_gains() static void report_gains()
{ {
Serial.printf_P(PSTR("Gains\n")); Serial.printf_P(PSTR("Gains\n"));
print_divider(); print_divider();
@ -867,7 +867,7 @@ void report_gains()
} }
*/ */
/*void report_xtrack() /*static void report_xtrack()
{ {
Serial.printf_P(PSTR("XTrack\n")); Serial.printf_P(PSTR("XTrack\n"));
print_divider(); print_divider();
@ -882,7 +882,7 @@ void report_gains()
} }
*/ */
/*void report_throttle() /*static void report_throttle()
{ {
Serial.printf_P(PSTR("Throttle\n")); Serial.printf_P(PSTR("Throttle\n"));
print_divider(); print_divider();
@ -900,7 +900,7 @@ void report_gains()
print_blanks(2); print_blanks(2);
}*/ }*/
void report_imu() static void report_imu()
{ {
Serial.printf_P(PSTR("IMU\n")); Serial.printf_P(PSTR("IMU\n"));
print_divider(); print_divider();
@ -910,7 +910,7 @@ void report_imu()
print_blanks(2); print_blanks(2);
} }
void report_compass() static void report_compass()
{ {
Serial.printf_P(PSTR("Compass\n")); Serial.printf_P(PSTR("Compass\n"));
print_divider(); print_divider();
@ -931,7 +931,7 @@ void report_compass()
print_blanks(2); print_blanks(2);
} }
void report_flight_modes() static void report_flight_modes()
{ {
Serial.printf_P(PSTR("Flight modes\n")); Serial.printf_P(PSTR("Flight modes\n"));
print_divider(); print_divider();
@ -943,7 +943,7 @@ void report_flight_modes()
} }
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
void report_heli() static void report_heli()
{ {
Serial.printf_P(PSTR("Heli\n")); Serial.printf_P(PSTR("Heli\n"));
print_divider(); print_divider();
@ -962,7 +962,7 @@ void report_heli()
print_blanks(2); print_blanks(2);
} }
void report_gyro() static void report_gyro()
{ {
Serial.printf_P(PSTR("External Gyro:\n")); Serial.printf_P(PSTR("External Gyro:\n"));
@ -981,7 +981,7 @@ void report_gyro()
// CLI utilities // CLI utilities
/***************************************************************************/ /***************************************************************************/
/*void /*static void
print_PID(PID * pid) print_PID(PID * pid)
{ {
Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, D:%4.2f, IMAX:%ld\n"), Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, D:%4.2f, IMAX:%ld\n"),
@ -992,7 +992,7 @@ print_PID(PID * pid)
} }
*/ */
void static void
print_radio_values() print_radio_values()
{ {
Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max); Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
@ -1005,20 +1005,20 @@ print_radio_values()
//Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max); //Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
} }
void static void
print_switch(byte p, byte m) print_switch(byte p, byte m)
{ {
Serial.printf_P(PSTR("Pos %d: "),p); Serial.printf_P(PSTR("Pos %d: "),p);
Serial.println(flight_mode_strings[m]); Serial.println(flight_mode_strings[m]);
} }
void static void
print_done() print_done()
{ {
Serial.printf_P(PSTR("\nSaved Settings\n\n")); Serial.printf_P(PSTR("\nSaved Settings\n\n"));
} }
void static void
print_blanks(int num) print_blanks(int num)
{ {
while(num > 0){ while(num > 0){
@ -1027,7 +1027,7 @@ print_blanks(int num)
} }
} }
void static void
print_divider(void) print_divider(void)
{ {
for (int i = 0; i < 40; i++) { for (int i = 0; i < 40; i++) {
@ -1037,7 +1037,7 @@ print_divider(void)
} }
// read at 50Hz // read at 50Hz
bool static bool
radio_input_switch(void) radio_input_switch(void)
{ {
static int8_t bouncer = 0; static int8_t bouncer = 0;
@ -1063,7 +1063,7 @@ radio_input_switch(void)
} }
void zero_eeprom(void) static void zero_eeprom(void)
{ {
byte b = 0; byte b = 0;
@ -1076,7 +1076,7 @@ void zero_eeprom(void)
Serial.printf_P(PSTR("done\n")); Serial.printf_P(PSTR("done\n"));
} }
void print_enabled(boolean b) static void print_enabled(boolean b)
{ {
if(b) if(b)
Serial.printf_P(PSTR("en")); Serial.printf_P(PSTR("en"));
@ -1085,7 +1085,7 @@ void print_enabled(boolean b)
Serial.printf_P(PSTR("abled\n")); Serial.printf_P(PSTR("abled\n"));
} }
void static void
print_accel_offsets(void) print_accel_offsets(void)
{ {
Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"), Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"),
@ -1094,7 +1094,7 @@ print_accel_offsets(void)
(float)imu.az()); (float)imu.az());
} }
void static void
print_gyro_offsets(void) print_gyro_offsets(void)
{ {
Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
@ -1105,7 +1105,7 @@ print_gyro_offsets(void)
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
RC_Channel * static RC_Channel *
heli_get_servo(int servo_num){ heli_get_servo(int servo_num){
if( servo_num == CH_1 ) if( servo_num == CH_1 )
return &g.heli_servo_1; return &g.heli_servo_1;
@ -1119,7 +1119,7 @@ heli_get_servo(int servo_num){
} }
// Used to read integer values from the serial port // Used to read integer values from the serial port
int read_num_from_serial() { static int read_num_from_serial() {
byte index = 0; byte index = 0;
byte timeout = 0; byte timeout = 0;
char data[5] = ""; char data[5] = "";

30
ArduCopterMega/system.pde
View File

@ -7,10 +7,10 @@ The init_ardupilot function processes everything we need for an in - air restart
*****************************************************************************/ *****************************************************************************/
// Functions called from the top-level menu // Functions called from the top-level menu
extern int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde
extern int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde
extern int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp
extern int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde static int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde
// This is the help function // This is the help function
// PSTR is an AVR macro to read strings from flash memory // PSTR is an AVR macro to read strings from flash memory
@ -42,7 +42,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
// Create the top-level menu object. // Create the top-level menu object.
MENU(main_menu, "AC 2.0.36 Beta", main_menu_commands); MENU(main_menu, "AC 2.0.36 Beta", main_menu_commands);
void init_ardupilot() static void init_ardupilot()
{ {
// Console serial port // Console serial port
// //
@ -262,7 +262,7 @@ void init_ardupilot()
//******************************************************************************** //********************************************************************************
//This function does all the calibrations, etc. that we need during a ground start //This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************** //********************************************************************************
void startup_ground(void) static void startup_ground(void)
{ {
gcs.send_text_P(SEVERITY_LOW,PSTR("GROUND START")); gcs.send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
@ -340,7 +340,7 @@ void startup_ground(void)
gcs.send_message(MSG_HEARTBEAT); gcs.send_message(MSG_HEARTBEAT);
} }
void set_mode(byte mode) static void set_mode(byte mode)
{ {
if(control_mode == mode){ if(control_mode == mode){
// don't switch modes if we are already in the correct mode. // don't switch modes if we are already in the correct mode.
@ -410,7 +410,7 @@ void set_mode(byte mode)
gcs.send_message(MSG_MODE_CHANGE); gcs.send_message(MSG_MODE_CHANGE);
} }
void set_failsafe(boolean mode) static void set_failsafe(boolean mode)
{ {
// only act on changes // only act on changes
// ------------------- // -------------------
@ -436,14 +436,14 @@ void set_failsafe(boolean mode)
void resetPerfData(void) { static void resetPerfData(void) {
mainLoop_count = 0; mainLoop_count = 0;
G_Dt_max = 0; G_Dt_max = 0;
gps_fix_count = 0; gps_fix_count = 0;
perf_mon_timer = millis(); perf_mon_timer = millis();
} }
void static void
init_compass() init_compass()
{ {
dcm.set_compass(&compass); dcm.set_compass(&compass);
@ -459,7 +459,7 @@ init_compass()
* be larger than HP or you'll be in big trouble! The smaller the gap, the more * be larger than HP or you'll be in big trouble! The smaller the gap, the more
* careful you need to be. Julian Gall 6 - Feb - 2009. * careful you need to be. Julian Gall 6 - Feb - 2009.
*/ */
unsigned long freeRAM() { static unsigned long freeRAM() {
uint8_t * heapptr, * stackptr; uint8_t * heapptr, * stackptr;
stackptr = (uint8_t *)malloc(4); // use stackptr temporarily stackptr = (uint8_t *)malloc(4); // use stackptr temporarily
heapptr = stackptr; // save value of heap pointer heapptr = stackptr; // save value of heap pointer
@ -468,13 +468,13 @@ unsigned long freeRAM() {
return stackptr - heapptr; return stackptr - heapptr;
} }
void static void
init_simple_bearing() init_simple_bearing()
{ {
initial_simple_bearing = dcm.yaw_sensor; initial_simple_bearing = dcm.yaw_sensor;
} }
void static void
init_throttle_cruise() init_throttle_cruise()
{ {
// are we moving from manual throttle to auto_throttle? // are we moving from manual throttle to auto_throttle?
@ -485,7 +485,7 @@ init_throttle_cruise()
#if BROKEN_SLIDER == 1 #if BROKEN_SLIDER == 1
boolean static boolean
check_startup_for_CLI() check_startup_for_CLI()
{ {
//return true; //return true;
@ -505,7 +505,7 @@ check_startup_for_CLI()
#else #else
boolean static boolean
check_startup_for_CLI() check_startup_for_CLI()
{ {
return (digitalRead(SLIDE_SWITCH_PIN) == 0); return (digitalRead(SLIDE_SWITCH_PIN) == 0);

8
ArduCopterMega/test.pde
View File

@ -80,7 +80,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
// A Macro to create the Menu // A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands); MENU(test_menu, "test", test_menu_commands);
int8_t static int8_t
test_mode(uint8_t argc, const Menu::arg *argv) test_mode(uint8_t argc, const Menu::arg *argv)
{ {
//Serial.printf_P(PSTR("Test Mode\n\n")); //Serial.printf_P(PSTR("Test Mode\n\n"));
@ -1026,12 +1026,12 @@ test_mission(uint8_t argc, const Menu::arg *argv)
return (0); return (0);
} }
void print_hit_enter() static void print_hit_enter()
{ {
Serial.printf_P(PSTR("Hit Enter to exit.\n\n")); Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));
} }
void fake_out_gps() static void fake_out_gps()
{ {
static float rads; static float rads;
g_gps->new_data = true; g_gps->new_data = true;
@ -1054,7 +1054,7 @@ void fake_out_gps()
void print_motor_out(){ static void print_motor_out(){
Serial.printf("out: R: %d, L: %d F: %d B: %d\n", Serial.printf("out: R: %d, L: %d F: %d B: %d\n",
(motor_out[CH_1] - g.rc_3.radio_min), (motor_out[CH_1] - g.rc_3.radio_min),
(motor_out[CH_2] - g.rc_3.radio_min), (motor_out[CH_2] - g.rc_3.radio_min),