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

2.0.29

Browse Source 
Fixed Mavlink issue
Added look at home option for SIMPLE mode. Off be default. Untested.
Upped performance of Yaw.


git-svn-id: https://arducopter.googlecode.com/svn/trunk@2695 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-06-27 17:01:53 +00:00
parent 418a4b3903
commit f9c402f0f3
9 changed files with 93 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopterMega/APM_Config.h
View File

@ -79,3 +79,9 @@
#define RE_LED AN14 // Mega PE5 pin, OUT6
#define RI_LED AN10 // Mega PH4 pin, OUT5
#define LE_LED AN8 // Mega PH5 pin, OUT4
// experimental!!
// Yaw is controled by targeting home. you will not have Yaw override.
// flying too close to home may induce spins.
#define SIMPLE_LOOK_AT_HOME 0

46
ArduCopterMega/ArduCopterMega.pde
View File

@ -219,7 +219,9 @@ GPS *g_gps;
#if SONAR_TYPE == MAX_SONAR_XL
AP_RangeFinder_MaxsonarXL sonar;//(SONAR_PORT, &adc);
#elif SONAR_TYPE == MAX_SONAR_LV
AP_RangeFinder_MaxsonarLV sonar;//(SONAR_PORT, &adc);
// XXX honestly I think these output the same values
// If someone knows, can they confirm it?
AP_RangeFinder_MaxsonarXL sonar;//(SONAR_PORT, &adc);
#endif
////////////////////////////////////////////////////////////////////////////////
@ -316,7 +318,6 @@ float cos_roll_x = 1;
float cos_pitch_x = 1;
float cos_yaw_x = 1;
float sin_pitch_y, sin_yaw_y, sin_roll_y;
float sin_nav_y, cos_nav_x; // used in calc_nav_output XXX move to local funciton
bool simple_bearing_is_set = false;
long initial_simple_bearing; // used for Simple mode
@ -707,7 +708,6 @@ void medium_loop()
Log_Write_Control_Tuning();
#endif
// XXX this should be a "GCS medium loop" interface
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(5,45);
// send all requested output streams with rates requested
@ -786,8 +786,6 @@ void fifty_hz_loop()
#endif
// XXX is it appropriate to be doing the comms below on the fast loop?
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT != GCS_PORT
// kick the HIL to process incoming sensor packets
hil.update();
@ -809,8 +807,6 @@ void fifty_hz_loop()
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(45,1000);
#endif
// XXX this should be absorbed into the above,
// or be a "GCS fast loop" interface
#if FRAME_CONFIG == TRI_FRAME
// Hack - had to move to 50hz loop to test a theory
@ -876,7 +872,6 @@ void slow_loop()
// blink if we are armed
update_lights();
// XXX this should be a "GCS slow loop" interface
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(1,5);
// send all requested output streams with rates requested
@ -919,7 +914,7 @@ void super_slow_loop()
if (g.log_bitmask & MASK_LOG_CURRENT)
Log_Write_Current();
gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz
gcs.send_message(MSG_HEARTBEAT);
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_HEARTBEAT);
@ -1122,13 +1117,27 @@ void update_current_flight_mode(void)
limit_nav_pitch_roll(4500);
}
// are we at rest? reset nav_yaw
if(g.rc_3.control_in == 0){
clear_yaw_control();
}else{
// Yaw control
output_manual_yaw();
}
#if SIMPLE_LOOK_AT_HOME == 0
// This is typical yaw behavior
// are we at rest? reset nav_yaw
if(g.rc_3.control_in == 0){
clear_yaw_control();
}else{
// Yaw control
output_manual_yaw();
}
#else
// This is experimental,
// copter will always point at home
if(home_is_set)
point_at_home_yaw();
// Output Pitch, Roll, Yaw and Throttle
// ------------------------------------
auto_yaw();
#endif
// apply throttle control
output_manual_throttle();
@ -1447,6 +1456,11 @@ void update_nav_yaw()
}
}
void point_at_home_yaw()
{
nav_yaw = get_bearing(&current_loc, &home);
}
void check_DCM()
{
#if DYNAMIC_DRIFT == 1

34
ArduCopterMega/Attitude.pde
View File

@ -303,7 +303,7 @@ output_yaw_with_hold(boolean hold)
yaw_error = wrap_180(yaw_error);
// limit the error we're feeding to the PID
yaw_error = constrain(yaw_error, -3500, 3500); // limit error to 60 degees
yaw_error = constrain(yaw_error, -4500, 4500); // limit error to 60 degees
// Apply PID and save the new angle back to RC_Channel
g.rc_4.servo_out = g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
@ -313,36 +313,4 @@ output_yaw_with_hold(boolean hold)
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2500, 2500); // limit error to 60 degees
}
#elif YAW_OPTION == 2
void
output_yaw_with_hold(boolean hold)
{
if(hold){
// try and hold the current nav_yaw setting
yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60°
// we need to wrap our value so we can be -180 to 180 (*100)
yaw_error = wrap_180(yaw_error);
// limit the error we're feeding to the PID
yaw_error = constrain(yaw_error, -3500, 3500); // limit error to 40 degees
// Apply PID and save the new angle back to RC_Channel
g.rc_4.servo_out = g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
// add in yaw dampener
g.rc_4.servo_out -= (degrees(omega.z) * 100) * g.pid_yaw.kD();
}else{
// RATE control
long error = ((long)g.rc_4.control_in * 6) - (degrees(omega.z) * 100); // control is += 4500 * 6 = 36000
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
nav_yaw = dcm.yaw_sensor; // save our Yaw
}
// Limit Output
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2500, 2500); // limit to 24°
}
#endif

1
ArduCopterMega/Camera.pde
View File

@ -32,7 +32,6 @@ camera_stabilization()
g.rc_camera_roll.calc_pwm();
APM_RC.OutputCh(CH_6, g.rc_camera_roll.radio_out);
//If you want to do control mixing use this function.
// set servo_out to the control input from radio
//rc_camera_roll = g.rc_2.control_mix(dcm.pitch_sensor);

46
ArduCopterMega/GCS_Mavlink.pde
View File

@ -49,7 +49,6 @@ GCS_MAVLINK::init(BetterStream * port)
streamRateExtra1.set_and_save(0);
streamRateExtra2.set_and_save(0);
streamRateExtra3.set_and_save(0);
}
void
@ -73,11 +72,6 @@ GCS_MAVLINK::update(void)
// Update packet drops counter
packet_drops += status.packet_rx_drop_count;
// send out queued params/ waypoints
_queued_send();
@ -200,7 +194,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode
mavlink_request_data_stream_t packet;
mavlink_msg_request_data_stream_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
if (mavlink_check_target(packet.target_system,packet.target_component))
break;
int freq = 0; // packet frequency
@ -214,15 +210,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
switch(packet.req_stream_id){
case MAV_DATA_STREAM_ALL:
streamRateRawSensors = freq;
streamRateExtendedStatus = freq;
streamRateRCChannels = freq;
streamRateRawController = freq;
streamRatePosition = freq;
streamRateExtra1 = freq;
streamRateExtra2 = freq;
streamRateRawSensors = freq;
streamRateExtendedStatus = freq;
streamRateRCChannels = freq;
streamRateRawController = freq;
streamRatePosition = freq;
streamRateExtra1 = freq;
streamRateExtra2 = freq;
//streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group.
streamRateExtra3 = freq; // Don't save!!
streamRateExtra3 = freq; // Don't save!!
break;
@ -404,12 +400,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
msg->compid,
g.waypoint_total + 1); // + home
waypoint_timelast_send = millis();
waypoint_sending = true;
waypoint_receiving = false;
waypoint_dest_sysid = msg->sysid;
waypoint_dest_compid = msg->compid;
requested_interface = chan;
waypoint_timelast_send = millis();
waypoint_sending = true;
waypoint_receiving = false;
waypoint_dest_sysid = msg->sysid;
waypoint_dest_compid = msg->compid;
requested_interface = chan;
break;
}
@ -419,13 +415,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
//send_text_P(SEVERITY_LOW,PSTR("waypoint request"));
// Check if sending waypiont
if (!waypoint_sending) break;
if (!waypoint_sending)
break;
// decode
mavlink_waypoint_request_t packet;
mavlink_msg_waypoint_request_decode(msg, &packet);
if (mavlink_check_target(packet.target_system,packet.target_component)) break;
if (mavlink_check_target(packet.target_system,packet.target_component))
break;
// send waypoint
tell_command = get_command_with_index(packet.seq);
@ -772,7 +770,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// find the requested parameter
vp = AP_Var::find(key);
if ((NULL != vp) && // exists
if ((NULL != vp) && // exists
!isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf

38
ArduCopterMega/config.h
View File

@ -357,19 +357,33 @@
//////////////////////////////////////////////////////////////////////////////
// YAW Control
//
#ifndef YAW_P
# define YAW_P 0.4 // increase for more aggressive Yaw Hold, decrease if it's bouncy
#if YAW_OPTION == 1 // 0 = hybrid rate approach, 1 = offset Yaw approach
#ifndef YAW_P
# define YAW_P 0.6 // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif
#ifndef YAW_I
# define YAW_I 0.005 // set to .0001 to try and get over user's steady state error caused by poor balance
#endif
#ifndef YAW_D
# define YAW_D 0.08 // .7 = almost no yaw Trying a lower value to prevent odd behavior
#endif
#ifndef YAW_IMAX
# define YAW_IMAX 1 // degrees * 100
#endif
#else
#ifndef YAW_P
# define YAW_P 0.4 // increase for more aggressive Yaw Hold, decrease if it's bouncy
#endif
#ifndef YAW_I
# define YAW_I 0.005 // set to .0001 to try and get over user's steady state error caused by poor balance
#endif
#ifndef YAW_D
# define YAW_D 0.08 // .7 = almost no yaw Trying a lower value to prevent odd behavior
#endif
#ifndef YAW_IMAX
# define YAW_IMAX 1 // degrees * 100
#endif
#endif
#ifndef YAW_I
# define YAW_I 0.005 // set to .0001 to try and get over user's steady state error caused by poor balance
#endif
#ifndef YAW_D
# define YAW_D 0.08 // .7 = almost no yaw Trying a lower value to prevent odd behavior
#endif
#ifndef YAW_IMAX
# define YAW_IMAX 1 // degrees * 100
#endif
//////////////////////////////////////////////////////////////////////////////
// Autopilot control limits
//

4
ArduCopterMega/navigation.pde
View File

@ -118,8 +118,8 @@ void calc_simple_nav()
void calc_nav_output()
{
// get the sin and cos of the bearing error - rotated 90°
sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100));
float sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100));
float cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100));
// rotate the vector
nav_roll = (float)nav_lat * cos_nav_x;

6
ArduCopterMega/radio.pde
View File

@ -22,7 +22,11 @@ void init_rc_in()
g.rc_1.dead_zone = 60; // 60 = .6 degrees
g.rc_2.dead_zone = 60;
g.rc_3.dead_zone = 60;
g.rc_4.dead_zone = 600;
#if YAW_OPTION == 1
g.rc_4.dead_zone = 500;// 1 = offset Yaw approach
#else
g.rc_4.dead_zone = 800;// 0 = hybrid rate approach
#endif
//set auxiliary ranges
g.rc_5.set_range(0,1000);

2
ArduCopterMega/system.pde
View File

@ -40,7 +40,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
};
// Create the top-level menu object.
MENU(main_menu, "AC 2.0.28 Beta", main_menu_commands);
MENU(main_menu, "AC 2.0.29 Beta", main_menu_commands);
void init_ardupilot()
{