Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

Browse Source
This commit is contained in:
Chris Anderson 2012-01-22 13:59:30 -08:00
commit 09c16508b3
27 changed files with 237 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduCopter/ArduCopter.pde
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduCopter V2.2 b3"
#define THISFIRMWARE "ArduCopter V2.2 b4"
/*
ArduCopter Version 2.2
Authors: Jason Short
@ -323,6 +323,9 @@ static int16_t y_actual_speed;
static int16_t x_rate_error;
static int16_t y_rate_error;
//static int16_t my_max_speed; // used for debugging logs
//static int16_t target_x_rate;
////////////////////////////////////////////////////////////////////////////////
// Radio
////////////////////////////////////////////////////////////////////////////////
@ -360,8 +363,6 @@ static bool rc_override_active = false;
// Status flag that tracks whether we are under GCS control
static uint32_t rc_override_fs_timer = 0;
////////////////////////////////////////////////////////////////////////////////
// Heli
////////////////////////////////////////////////////////////////////////////////
@ -604,9 +605,9 @@ static int16_t landing_boost;
////////////////////////////////////////////////////////////////////////////////
// The location of the copter in relation to home, updated every GPS read
static int32_t home_to_copter_bearing;
// distance between plane and home in meters (not cm!!!)
// distance between plane and home in cm
static int32_t home_distance;
// distance between plane and next_WP in meters (not cm!!!)
// distance between plane and next_WP in cm
static int32_t wp_distance;
////////////////////////////////////////////////////////////////////////////////
@ -1532,7 +1533,7 @@ void update_throttle_mode(void)
takeoff_complete = false;
// reset baro data if we are near home
if(home_distance < 4 || GPS_enabled == false){
if(home_distance < 400 || GPS_enabled == false){ // 4m from home
// causes Baro to do a quick recalibration
// XXX commented until further testing
// reset_baro();
@ -1602,7 +1603,7 @@ void update_throttle_mode(void)
// get the AP throttle, if landing boost > 0 we are actually Landing
// This allows us to grab just the compensation.
if(landing_boost > 0)
nav_throttle = get_nav_throttle(-100);
nav_throttle = get_nav_throttle(-200);
else
nav_throttle = get_nav_throttle(altitude_error);
@ -1743,7 +1744,7 @@ static void update_navigation()
// are we in SIMPLE mode?
if(do_simple && g.super_simple){
// get distance to home
if(home_distance > 10){ // 10m from home
if(home_distance > 1000){ // 10m from home
// we reset the angular offset to be a vector from home to the quad
initial_simple_bearing = home_to_copter_bearing;
//Serial.printf("ISB: %d\n", initial_simple_bearing);

3
ArduCopter/Attitude.pde
View File

@ -165,7 +165,8 @@ get_nav_throttle(int32_t z_error)
//output -= constrain(rate_d, -25, 25);
// light filter of output
output = (old_output * 3 + output) / 4;
//output = (old_output * 3 + output) / 4;
output = (old_output + output) / 2;
// save our output
old_output = output;

2
ArduCopter/Camera.pde
View File

@ -4,6 +4,8 @@
static void init_camera()
{
APM_RC.enable_out(CH_CAM_PITCH);
APM_RC.enable_out(CH_CAM_ROLL);
// ch 6 high(right) is down.
g.rc_camera_pitch.set_angle(4500);
g.rc_camera_roll.set_angle(4500);

2
ArduCopter/GCS_Mavlink copy.txt
View File

@ -270,7 +270,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
nav_pitch / 1.0e2,
target_bearing / 1.0e2,
dcm.yaw_sensor / 1.0e2, // was target_bearing
wp_distance,
wp_distance / 1.0e2,
altitude_error / 1.0e2,
0,
crosstrack_error);

11
ArduCopter/GCS_Mavlink.pde
View File

@ -118,7 +118,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
nav_pitch / 1.0e2,
nav_bearing / 1.0e2,
target_bearing / 1.0e2,
wp_distance,
wp_distance / 1.0e2,
altitude_error / 1.0e2,
0,
crosstrack_error); // was 0
@ -1518,15 +1518,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// set dcm hil sensor
dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed);
// rad/sec
// rad/sec
/*
Vector3f gyros;
gyros.x = (float)packet.rollspeed;
gyros.y = (float)packet.pitchspeed;
gyros.z = (float)packet.yawspeed;
imu.set_gyro(gyros);
imu.set_gyro(gyros);
*/
//imu.set_accel(accels);
break;
}

18
ArduCopter/Log.pde
View File

@ -528,16 +528,28 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt(wp_distance); // 1
DataFlash.WriteInt(target_bearing/100); // 2
DataFlash.WriteInt(nav_bearing/100); // 2
DataFlash.WriteInt(long_error); // 3
DataFlash.WriteInt(lat_error); // 4
DataFlash.WriteInt(nav_lon); // 5
DataFlash.WriteInt(nav_lat); // 6
DataFlash.WriteInt(g.pi_nav_lon.get_integrator()); // 7
DataFlash.WriteInt(g.pi_nav_lat.get_integrator()); // 8
DataFlash.WriteInt(x_actual_speed); // 7
DataFlash.WriteInt(y_actual_speed); // 8
DataFlash.WriteInt(g.pi_loiter_lon.get_integrator()); // 9
DataFlash.WriteInt(g.pi_loiter_lat.get_integrator()); // 10
/*DataFlash.WriteInt(wp_distance); // 1
DataFlash.WriteInt(nav_bearing/100); // 2
DataFlash.WriteInt(my_max_speed); // 3
DataFlash.WriteInt(long_error); // 4
DataFlash.WriteInt(x_actual_speed); // 5
DataFlash.WriteInt(target_x_rate); // 6
DataFlash.WriteInt(x_rate_error); // 7
DataFlash.WriteInt(nav_lon_p); // 8
DataFlash.WriteInt(g.pi_loiter_lon.get_integrator()); // 9
DataFlash.WriteInt(nav_lon); // 10
*/
DataFlash.WriteByte(END_BYTE);
}

14
ArduCopter/Parameters.h
View File

@ -107,7 +107,7 @@ public:
k_param_optflow_enabled,
k_param_low_voltage,
k_param_ch7_option,
k_param_sonar_type,
k_param_sonar_type,
k_param_super_simple, //155
//
@ -167,7 +167,7 @@ public:
//
// 235: PI/D Controllers
//
k_param_stablize_d = 234,
k_param_stabilize_d = 234,
k_param_pi_rate_roll = 235,
k_param_pi_rate_pitch,
k_param_pi_rate_yaw,
@ -184,7 +184,7 @@ public:
k_param_pi_acro_pitch,
k_param_pi_optflow_roll,
k_param_pi_optflow_pitch, // 250
k_param_loiter_d,
// 254,255: reserved
};
@ -286,6 +286,7 @@ public:
AP_Float camera_pitch_gain;
AP_Float camera_roll_gain;
AP_Float stablize_d;
AP_Float loiter_d;
// PI/D controllers
APM_PI pi_rate_roll;
@ -341,10 +342,10 @@ public:
command_total (0, k_param_command_total, PSTR("WP_TOTAL")),
command_index (0, k_param_command_index, PSTR("WP_INDEX")),
command_nav_index (0, k_param_command_nav_index, PSTR("WP_MUST_INDEX")),
waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")),
waypoint_radius (WP_RADIUS_DEFAULT * 100, k_param_waypoint_radius, PSTR("WP_RADIUS")),
loiter_radius (LOITER_RADIUS, k_param_loiter_radius, PSTR("WP_LOITER_RAD")),
waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")),
crosstrack_gain (CROSSTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
crosstrack_gain (CROSSTRACK_GAIN, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")),
auto_land_timeout (AUTO_LAND_TIME * 1000, k_param_auto_land_timeout, PSTR("AUTO_LAND")),
throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")),
@ -409,7 +410,8 @@ public:
camera_pitch_gain (CAM_PITCH_GAIN, k_param_camera_pitch_gain, PSTR("CAM_P_G")),
camera_roll_gain (CAM_ROLL_GAIN, k_param_camera_roll_gain, PSTR("CAM_R_G")),
stablize_d (STABILIZE_D, k_param_stablize_d, PSTR("STAB_D")),
stablize_d (STABILIZE_D, k_param_stabilize_d, PSTR("STAB_D")),
loiter_d (LOITER_D, k_param_loiter_d, PSTR("LOITER_D")),
// PI controller group key name initial P initial I initial imax
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------

2
ArduCopter/commands.pde
View File

@ -178,7 +178,7 @@ static void set_next_WP(struct Location *wp)
// reset speed governer
// --------------------
waypoint_speed_gov = 0;
waypoint_speed_gov = WAYPOINT_SPEED_MIN;
}

4
ArduCopter/commands_logic.pde
View File

@ -365,7 +365,7 @@ static bool verify_land()
//landing_boost = min(landing_boost, 15);
float tmp = (1.75 * icount * icount) - (7.2 * icount);
landing_boost = tmp;
landing_boost = constrain(landing_boost, 0, 200);
landing_boost = constrain(landing_boost, 1, 200);
icount += 0.4;
//Serial.printf("lb:%d, %1.4f, ic:%1.4f\n",landing_boost, tmp, icount);
@ -512,7 +512,7 @@ static void do_change_alt()
static void do_within_distance()
{
condition_value = command_cond_queue.lat;
condition_value = command_cond_queue.lat * 100;
}
static void do_yaw()

13
ArduCopter/config.h
View File

@ -622,20 +622,23 @@
# define LOITER_P 1.5 // was .25 in previous
#endif
#ifndef LOITER_I
# define LOITER_I 0.04 // Wind control
# define LOITER_I 0.09 // Wind control
#endif
#ifndef LOITER_IMAX
# define LOITER_IMAX 30 // degrees°
#endif
#ifndef LOITER_D
# define LOITER_D 3 // rate control
#endif
//////////////////////////////////////////////////////////////////////////////
// WP Navigation control gains
//
#ifndef NAV_P
# define NAV_P 2.2 // 3 was causing rapid oscillations in Loiter
# define NAV_P 3.0 // 3 was causing rapid oscillations in Loiter
#endif
#ifndef NAV_I
# define NAV_I 0.15 // used in traverals
# define NAV_I 0 //
#endif
#ifndef NAV_IMAX
# define NAV_IMAX 30 // degrees
@ -645,6 +648,10 @@
# define WAYPOINT_SPEED_MAX 600 // for 6m/s error = 13mph
#endif
#ifndef WAYPOINT_SPEED_MIN
# define WAYPOINT_SPEED_MIN 100 // for 6m/s error = 13mph
#endif
//////////////////////////////////////////////////////////////////////////////
// Throttle control gains

10
ArduCopter/motors_hexa.pde
View File

@ -10,6 +10,16 @@ static void init_motors_out()
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
}
static void output_motors_armed()
{
int roll_out, pitch_out;

12
ArduCopter/motors_octa.pde
View File

@ -10,6 +10,18 @@ static void init_motors_out()
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
APM_RC.enable_out(MOT_7);
APM_RC.enable_out(MOT_8);
}
static void output_motors_armed()
{
int roll_out, pitch_out;

12
ArduCopter/motors_octa_quad.pde
View File

@ -10,6 +10,18 @@ static void init_motors_out()
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
APM_RC.enable_out(MOT_7);
APM_RC.enable_out(MOT_8);
}
static void output_motors_armed()
{
int roll_out, pitch_out;

8
ArduCopter/motors_quad.pde
View File

@ -9,6 +9,14 @@ static void init_motors_out()
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
}
static void output_motors_armed()
{
int roll_out, pitch_out;

7
ArduCopter/motors_tri.pde
View File

@ -8,6 +8,13 @@ static void init_motors_out()
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_4);
}
static void output_motors_armed()
{

10
ArduCopter/motors_y6.pde
View File

@ -12,6 +12,16 @@ static void init_motors_out()
#endif
}
static void motors_output_enable()
{
APM_RC.enable_out(MOT_1);
APM_RC.enable_out(MOT_2);
APM_RC.enable_out(MOT_3);
APM_RC.enable_out(MOT_4);
APM_RC.enable_out(MOT_5);
APM_RC.enable_out(MOT_6);
}
static void output_motors_armed()
{
int out_min = g.rc_3.radio_min;

40
ArduCopter/navigation.pde
View File

@ -143,13 +143,13 @@ static void calc_location_error(struct Location *next_loc)
*/
#define NAV_ERR_MAX 800
static void calc_loiter1(int x_error, int y_error)
static void calc_loiter(int x_error, int y_error)
{
int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav);
int16_t lon_D = 3 * x_actual_speed ; // this controls the small bumps
int16_t lon_D = g.loiter_d * x_actual_speed ; // this controls the small bumps
int16_t lat_PI = g.pi_loiter_lat.get_pi(y_error, dTnav);
int16_t lat_D = 3 * y_actual_speed ;
int16_t lat_D = g.loiter_d * y_actual_speed ;
//limit of terms
lon_D = constrain(lon_D,-500,500);
@ -160,14 +160,15 @@ static void calc_loiter1(int x_error, int y_error)
}
static void calc_loiter(int x_error, int y_error)
static void calc_loiter1(int x_error, int y_error)
{
// East/West
x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); //800
int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error);
int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav);
x_rate_error = x_target_speed - x_actual_speed;
nav_lon_p = g.pi_nav_lon.get_p(x_rate_error);
nav_lon_p = x_rate_error * g.loiter_d;
nav_lon_p = constrain(nav_lon_p, -1200, 1200);
nav_lon = nav_lon_p + x_iterm;
nav_lon = constrain(nav_lon, -2500, 2500);
@ -177,7 +178,8 @@ static void calc_loiter(int x_error, int y_error)
int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error);
int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav);
y_rate_error = y_target_speed - y_actual_speed;
nav_lat_p = g.pi_nav_lat.get_p(y_rate_error);
nav_lat_p = y_rate_error * g.loiter_d;
nav_lat_p = constrain(nav_lat_p, -1200, 1200);
nav_lat = nav_lat_p + y_iterm;
nav_lat = constrain(nav_lat, -2500, 2500);
@ -270,21 +272,20 @@ static int16_t calc_desired_speed(int16_t max_speed)
*/
// max_speed is default 600 or 6m/s
// (wp_distance * 50) = 1/2 of the distance converted to speed
// (wp_distance * .5) = 1/2 of the distance converted to speed
// wp_distance is always in m/s and not cm/s - I know it's stupid that way
// for example 4m from target = 200cm/s speed
// we choose the lowest speed based on disance
max_speed = min(max_speed, (wp_distance * 100));
max_speed = min(max_speed, wp_distance);
// go at least 100cm/s
max_speed = max(max_speed, WAYPOINT_SPEED_MIN);
// limit the ramp up of the speed
// waypoint_speed_gov is reset to 0 at each new WP command
if(waypoint_speed_gov < max_speed){
if(max_speed > waypoint_speed_gov){
waypoint_speed_gov += (int)(100.0 * dTnav); // increase at .5/ms
// go at least 100cm/s
max_speed = max(100, waypoint_speed_gov);
// limit with governer
max_speed = min(max_speed, waypoint_speed_gov);
max_speed = waypoint_speed_gov;
}
return max_speed;
@ -296,7 +297,7 @@ static void calc_nav_rate(int max_speed)
update_crosstrack();
// nav_bearing includes crosstrack
float temp = (9000 - nav_bearing) * RADX100;
float temp = (9000l - nav_bearing) * RADX100;
x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413
x_rate_error = constrain(x_rate_error, -1000, 1000);
@ -398,7 +399,6 @@ static void set_new_altitude(int32_t _new_alt)
alt_change_flag = REACHED_ALT;
//Serial.printf("reached alt\n");
}
//Serial.printf("new alt: %d Org alt: %d\n", target_altitude, original_altitude);
}
@ -490,16 +490,14 @@ static int32_t wrap_180(int32_t error)
return current_loc.alt - home.alt;
}
*/
// distance is returned in meters
static int32_t get_distance(struct Location *loc1, struct Location *loc2)
{
//if(loc1->lat == 0 || loc1->lng == 0)
// return -1;
//if(loc2->lat == 0 || loc2->lng == 0)
// return -1;
float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
dlong = sqrt(sq(dlat) + sq(dlong)) * 1.113195;
return dlong;
}
/*
//static int32_t get_alt_distance(struct Location *loc1, struct Location *loc2)

3
ArduCopter/radio.pde
View File

@ -100,6 +100,7 @@ static void init_rc_out()
void output_min()
{
motors_output_enable();
#if FRAME_CONFIG == HELI_FRAME
heli_move_servos_to_mid();
#else
@ -158,7 +159,7 @@ static void throttle_failsafe(uint16_t pwm)
// Don't enter Failsafe if we are not armed
// home distance is in meters
// This is to prevent accidental RTL
if((motor_armed == true) && (home_distance > 10) && (current_loc.alt > 400)){
if((motor_armed == true) && (home_distance > 1000) && (current_loc.alt > 400)){
SendDebug("MSG FS ON ");
SendDebugln(pwm, DEC);
set_failsafe(true);

1
ArduCopter/setup.pde
View File

@ -1096,6 +1096,7 @@ static void print_enabled(boolean b)
static void
init_esc()
{
motors_output_enable();
while(1){
read_radio();
delay(100);

21
ArduPlane/radio.pde
View File

@ -33,18 +33,17 @@ static void init_rc_in()
static void init_rc_out()
{
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min);
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim);
APM_RC.OutputCh(CH_5, g.rc_5.radio_trim);
APM_RC.OutputCh(CH_6, g.rc_6.radio_trim);
APM_RC.OutputCh(CH_7, g.rc_7.radio_trim);
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
APM_RC.Init( &isr_registry ); // APM Radio initialization
APM_RC.enable_out(CH_1);
APM_RC.enable_out(CH_2);
APM_RC.enable_out(CH_3);
APM_RC.enable_out(CH_4);
APM_RC.enable_out(CH_5);
APM_RC.enable_out(CH_6);
APM_RC.enable_out(CH_7);
APM_RC.enable_out(CH_8);
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min);
@ -53,7 +52,7 @@ static void init_rc_out()
APM_RC.OutputCh(CH_5, g.rc_5.radio_trim);
APM_RC.OutputCh(CH_6, g.rc_6.radio_trim);
APM_RC.OutputCh(CH_7, g.rc_7.radio_trim);
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
}
static void read_radio()

3
libraries/APM_RC/APM_RC.h
View File

@ -41,6 +41,9 @@ class APM_RC_Class
virtual void clearOverride(void) = 0;
virtual void Force_Out() = 0;
virtual void SetFastOutputChannels( uint32_t channelmask ) = 0;
virtual void enable_out(uint8_t) = 0;
virtual void disable_out(uint8_t) = 0;
};

42
libraries/APM_RC/APM_RC_APM1.cpp
View File

@ -84,7 +84,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
pinMode(13,OUTPUT); //OUT3 (PB7/OC1C)
//Remember the registers not declared here remains zero by default...
TCCR1A =((1<<WGM11)|(1<<COM1A1)|(1<<COM1B1)|(1<<COM1C1)); //Please read page 131 of DataSheet, we are changing the registers settings of WGM11,COM1B1,COM1A1 to 1 thats all...
TCCR1A =((1<<WGM11)); //Please read page 131 of DataSheet, we are changing the registers settings of WGM11,COM1B1,COM1A1 to 1 thats all...
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11); //Prescaler set to 8, that give us a resolution of 0.5us, read page 134 of data sheet
OCR1A = 0xFFFF; // Init ODR registers to nil output signal
OCR1B = 0xFFFF;
@ -95,7 +95,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
pinMode(2,OUTPUT); //OUT7 (PE4/OC3B)
pinMode(3,OUTPUT); //OUT6 (PE5/OC3C)
pinMode(5,OUTPUT); //OUT10(PE3/OC3A)
TCCR3A =((1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1));
TCCR3A =((1<<WGM31));
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
OCR3A = 0xFFFF; // Init ODR registers to nil output signal
OCR3B = 0xFFFF;
@ -107,7 +107,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
pinMode(45,OUTPUT); //OUT0 (PL4/OC5B)
pinMode(46,OUTPUT); //OUT8 (PL3/OC5A)
TCCR5A =((1<<WGM51)|(1<<COM5A1)|(1<<COM5B1)|(1<<COM5C1));
TCCR5A =((1<<WGM51));
TCCR5B = (1<<WGM53)|(1<<WGM52)|(1<<CS51);
OCR5A = 0xFFFF; // Init ODR registers to nil output signal
OCR5B = 0xFFFF;
@ -119,7 +119,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg )
pinMode(7,OUTPUT); //OUT5 (PH4/OC4B)
pinMode(8,OUTPUT); //OUT4 (PH5/OC4C)
TCCR4A =((1<<WGM40)|(1<<WGM41)|(1<<COM4C1)|(1<<COM4B1)|(1<<COM4A1));
TCCR4A =((1<<WGM40)|(1<<WGM41));
//Prescaler set to 8, that give us a resolution of 0.5us
// Input Capture rising edge
TCCR4B = ((1<<WGM43)|(1<<WGM42)|(1<<CS41)|(1<<ICES4));
@ -153,6 +153,40 @@ void APM_RC_APM1::OutputCh(uint8_t ch, uint16_t pwm)
}
}
void APM_RC_APM1::enable_out(uint8_t ch)
{
switch(ch){
case 0: TCCR5A |= (1<<COM5B1); break; // CH_1 : OC5B
case 1: TCCR5A |= (1<<COM5C1); break; // CH_2 : OC5C
case 2: TCCR1A |= (1<<COM1B1); break; // CH_3 : OC1B
case 3: TCCR1A |= (1<<COM1C1); break; // CH_4 : OC1C
case 4: TCCR4A |= (1<<COM4C1); break; // CH_5 : OC4C
case 5: TCCR4A |= (1<<COM4B1); break; // CH_6 : OC4B
case 6: TCCR3A |= (1<<COM3C1); break; // CH_7 : OC3C
case 7: TCCR3A |= (1<<COM3B1); break; // CH_8 : OC3B
case 8: TCCR5A |= (1<<COM5A1); break; // CH_9 : OC5A
case 9: TCCR1A |= (1<<COM1A1); break; // CH_10: OC1A
case 10: TCCR3A |= (1<<COM3A1); break; // CH_11: OC3A
}
}
void APM_RC_APM1::disable_out(uint8_t ch)
{
switch(ch){
case 0: TCCR5A &= ~(1<<COM5B1); break; // CH_1 : OC5B
case 1: TCCR5A &= ~(1<<COM5C1); break; // CH_2 : OC5C
case 2: TCCR1A &= ~(1<<COM1B1); break; // CH_3 : OC1B
case 3: TCCR1A &= ~(1<<COM1C1); break; // CH_4 : OC1C
case 4: TCCR4A &= ~(1<<COM4C1); break; // CH_5 : OC4C
case 5: TCCR4A &= ~(1<<COM4B1); break; // CH_6 : OC4B
case 6: TCCR3A &= ~(1<<COM3C1); break; // CH_7 : OC3C
case 7: TCCR3A &= ~(1<<COM3B1); break; // CH_8 : OC3B
case 8: TCCR5A &= ~(1<<COM5A1); break; // CH_9 : OC5A
case 9: TCCR1A &= ~(1<<COM1A1); break; // CH_10: OC1A
case 10: TCCR3A &= ~(1<<COM3A1); break; // CH_11: OC3A
}
}
uint16_t APM_RC_APM1::InputCh(uint8_t ch)
{
uint16_t result;

3
libraries/APM_RC/APM_RC_APM1.h
View File

@ -20,6 +20,9 @@ class APM_RC_APM1 : public APM_RC_Class
void Force_Out(void);
void SetFastOutputChannels(uint32_t chmask);
void enable_out(uint8_t);
void disable_out(uint8_t);
void Force_Out0_Out1(void);
void Force_Out2_Out3(void);
void Force_Out6_Out7(void);

37
libraries/APM_RC/APM_RC_APM2.cpp
View File

@ -80,9 +80,8 @@ void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg )
pinMode(11,OUTPUT); // OUT2 (PB5/OC1A)
// WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR1.
// COM1A and COM1B enabled, set to low level on match.
// CS11: prescale by 8 => 0.5us tick
TCCR1A =((1<<WGM11)|(1<<COM1A1)|(1<<COM1B1));
TCCR1A =((1<<WGM11));
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
ICR1 = 40000; // 0.5us tick => 50hz freq
OCR1A = 0xFFFF; // Init OCR registers to nil output signal
@ -94,9 +93,8 @@ void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg )
pinMode(6,OUTPUT); // OUT5 (PH3/OC4A)
// WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR4.
// COM4A, 4B, 4C enabled, set to low level on match.
// CS41: prescale by 8 => 0.5us tick
TCCR4A =((1<<WGM41)|(1<<COM4A1)|(1<<COM4B1)|(1<<COM4C1));
TCCR4A =((1<<WGM41));
TCCR4B = (1<<WGM43)|(1<<WGM42)|(1<<CS41);
OCR4A = 0xFFFF; // Init OCR registers to nil output signal
OCR4B = 0xFFFF;
@ -109,9 +107,8 @@ void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg )
pinMode(5,OUTPUT); // OUT8 (PE3/OC3A)
// WGM: 1 1 1 0. Clear timer on Compare, TOP is ICR3
// COM3A, 3B, 3C enabled, set to low level on match
// CS31: prescale by 8 => 0.5us tick
TCCR3A =((1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1));
TCCR3A =((1<<WGM31));
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
OCR3A = 0xFFFF; // Init OCR registers to nil output signal
OCR3B = 0xFFFF;
@ -155,6 +152,34 @@ void APM_RC_APM2::OutputCh(unsigned char ch, uint16_t pwm)
}
}
void APM_RC_APM2::enable_out(uint8_t ch)
{
switch(ch) {
case 0: TCCR1A |= (1<<COM1B1); break; // CH_1 : OC1B
case 1: TCCR1A |= (1<<COM1A1); break; // CH_2 : OC1A
case 2: TCCR4A |= (1<<COM4C1); break; // CH_3 : OC4C
case 3: TCCR4A |= (1<<COM4B1); break; // CH_4 : OC4B
case 4: TCCR4A |= (1<<COM4A1); break; // CH_5 : OC4A
case 5: TCCR3A |= (1<<COM3C1); break; // CH_6 : OC3C
case 6: TCCR3A |= (1<<COM3B1); break; // CH_7 : OC3B
case 7: TCCR3A |= (1<<COM3A1); break; // CH_8 : OC3A
}
}
void APM_RC_APM2::disable_out(uint8_t ch)
{
switch(ch) {
case 0: TCCR1A &= ~(1<<COM1B1); break; // CH_1 : OC1B
case 1: TCCR1A &= ~(1<<COM1A1); break; // CH_2 : OC1A
case 2: TCCR4A &= ~(1<<COM4C1); break; // CH_3 : OC4C
case 3: TCCR4A &= ~(1<<COM4B1); break; // CH_4 : OC4B
case 4: TCCR4A &= ~(1<<COM4A1); break; // CH_5 : OC4A
case 5: TCCR3A &= ~(1<<COM3C1); break; // CH_6 : OC3C
case 6: TCCR3A &= ~(1<<COM3B1); break; // CH_7 : OC3B
case 7: TCCR3A &= ~(1<<COM3A1); break; // CH_8 : OC3A
}
}
uint16_t APM_RC_APM2::InputCh(unsigned char ch)
{
uint16_t result;

3
libraries/APM_RC/APM_RC_APM2.h
View File

@ -22,6 +22,9 @@ class APM_RC_APM2 : public APM_RC_Class
void Force_Out(void);
void SetFastOutputChannels(uint32_t chmask);
void enable_out(uint8_t);
void disable_out(uint8_t);
void Force_Out0_Out1(void);
void Force_Out2_Out3(void);
void Force_Out6_Out7(void);

1
libraries/PID/examples/pid/Makefile
View File

@ -1,2 +1 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk

14
libraries/PID/examples/pid/pid.pde
View File

@ -4,20 +4,30 @@
*/
#include <FastSerial.h>
#include <avr/pgmspace.h>
#include <AP_Common.h>
#include <APM_RC.h> // ArduPilot RC Library
#include <PID.h> // ArduPilot Mega RC Library
#include <Arduino_Mega_ISR_Registry.h>
long radio_in;
long radio_trim;
PID pid(10, "TEST1_");
Arduino_Mega_ISR_Registry isr_registry;
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
APM_RC_APM2 APM_RC;
#else
APM_RC_APM1 APM_RC;
#endif
PID pid(AP_Var::k_key_none, NULL);
void setup()
{
Serial.begin(38400);
Serial.println("ArduPilot Mega PID library test");
APM_RC.Init(); // APM Radio initialization
APM_RC.Init(&isr_registry); // APM Radio initialization
delay(1000);
//rc.trim();