aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Better sonar hold ++

Browse Source 
added mag calibration - still not sure if needed.
better sonar spike filter
2.0.19

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2434 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-29 19:32:55 +00:00
parent 1d21056f21
commit f747e39062
9 changed files with 52 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopterMega/APM_Config.h
View File

@ -7,7 +7,7 @@
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
#define NAV_TEST 1 // 0 = traditional, 1 = rate controlled
#define YAW_OPTION 0 // 0 = hybrid rate approach, 1 = offset Yaw approach
#define YAW_OPTION 1 // 0 = hybrid rate approach, 1 = offset Yaw approach
#define AUTO_RESET_LOITER 0 // enables Loiter to reset it's current location based on stick input.
#define FRAME_CONFIG QUAD_FRAME

10
ArduCopterMega/ArduCopterMega.pde
View File

@ -642,7 +642,7 @@ void medium_loop()
if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning();
if (g.log_bitmask & MASK_LOG_GPS){
if (GPS_enabled && g.log_bitmask & MASK_LOG_GPS){
Log_Write_GPS();
}
@ -1230,13 +1230,11 @@ void update_alt()
// filter out offset
// read barometer
baro_alt = read_barometer();
// XXX temp removed fr debugging
//filter out bad sonar reads
baro_alt = read_barometer();
int temp_sonar = sonar.read();
if(abs(temp_sonar - sonar_alt) < 300){
// spike filter
if((temp_sonar - sonar_alt) < 50){
sonar_alt = temp_sonar;
}

6
ArduCopterMega/Attitude.pde
View File

@ -132,15 +132,15 @@ void calc_nav_throttle()
if(error < 0){
// try and prevent rapid fall
//scaler = (altitude_sensor == BARO) ? 1 : 1;
scaler = (altitude_sensor == BARO) ? .8 : .8;
}
if(altitude_sensor == BARO){
nav_throttle = g.pid_baro_throttle.get_pid(error, delta_ms_medium_loop, scaler); // .2
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -35, 80);
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -30, 80);
}else{
nav_throttle = g.pid_sonar_throttle.get_pid(error, delta_ms_medium_loop, scaler); // .5
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -70, 100);
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -40, 100);
}
// simple filtering

6
ArduCopterMega/GCS.h
View File

@ -110,8 +110,10 @@ public:
// used by data_stream_send
static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax)
{
if (freq != 0 && freq >= freqMin && freq < freqMax) return true;
else return false;
if (freq != 0 && freq >= freqMin && freq < freqMax)
return true;
else
return false;
}
// send streams which match frequency range

54
ArduCopterMega/GCS_Mavlink.pde
View File

@ -87,40 +87,42 @@ void
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
{
if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
if (freqLoopMatch(streamRateRawSensors,freqMin,freqMax))
send_message(MSG_RAW_IMU);
if (freqLoopMatch(streamRateExtendedStatus,freqMin,freqMax)) {
send_message(MSG_EXTENDED_STATUS);
send_message(MSG_GPS_STATUS);
send_message(MSG_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
send_message(MSG_RAW_IMU);
}
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
send_message(MSG_EXTENDED_STATUS);
send_message(MSG_GPS_STATUS);
send_message(MSG_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
send_message(MSG_NAV_CONTROLLER_OUTPUT);
}
}
if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) {
send_message(MSG_LOCATION);
}
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
send_message(MSG_LOCATION);
}
if (freqLoopMatch(streamRateRawController,freqMin,freqMax)) {
// This is used for HIL. Do not change without discussing with HIL maintainers
send_message(MSG_SERVO_OUT);
}
if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
// This is used for HIL. Do not change without discussing with HIL maintainers
send_message(MSG_SERVO_OUT);
}
if (freqLoopMatch(streamRateRCChannels,freqMin,freqMax)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
}
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
}
if (freqLoopMatch(streamRateExtra1,freqMin,freqMax)){ // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE);
}
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE);
}
if (freqLoopMatch(streamRateExtra2,freqMin,freqMax)){ // Use Extra 2 for additional HIL info
send_message(MSG_VFR_HUD);
}
if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
send_message(MSG_VFR_HUD);
}
if (freqLoopMatch(streamRateExtra3,freqMin,freqMax)){
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
// Available datastream
}
}

10
ArduCopterMega/Log.pde
View File

@ -482,15 +482,15 @@ void Log_Write_Control_Tuning()
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
// Control
DataFlash.WriteInt((int)(g.rc_4.control_in));
DataFlash.WriteInt((int)(g.rc_4.servo_out));
DataFlash.WriteInt((int)(g.rc_4.control_in/100));
DataFlash.WriteInt((int)(g.rc_4.servo_out/100));
// yaw
DataFlash.WriteByte(yaw_debug);
DataFlash.WriteInt((int)(dcm.yaw_sensor/100));
DataFlash.WriteInt((int)(nav_yaw/100));
DataFlash.WriteInt((int)yaw_error/100);
DataFlash.WriteInt((int)(omega.z * 1000));
DataFlash.WriteInt((int)(omega.z * 100));
// Alt hold
DataFlash.WriteInt((int)(g.rc_3.servo_out));
@ -499,7 +499,7 @@ void Log_Write_Control_Tuning()
DataFlash.WriteInt((int)next_WP.alt); //
DataFlash.WriteInt((int)altitude_error); //
DataFlash.WriteInt((int)g.pid_baro_throttle.get_integrator());
DataFlash.WriteInt((int)g.pid_sonar_throttle.get_integrator());
DataFlash.WriteByte(END_BYTE);
}
@ -521,7 +521,7 @@ void Log_Read_Control_Tuning()
DataFlash.ReadInt(),
DataFlash.ReadInt(),
DataFlash.ReadInt(),
(float)DataFlash.ReadInt() / 1000.0,// Gyro Rate
(float)DataFlash.ReadInt(),// Gyro Rate
// Alt Hold
DataFlash.ReadInt(),

10
ArduCopterMega/config.h
View File

@ -360,7 +360,7 @@
# define YAW_I 0.01 // 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.05 // Trying a lower value to prevent odd behavior
# 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
@ -438,16 +438,16 @@
#ifndef THROTTLE_SONAR_P
# define THROTTLE_SONAR_P 0.5 //
# define THROTTLE_SONAR_P 0.35 // lowering P by .15
#endif
#ifndef THROTTLE_SONAR_I
# define THROTTLE_SONAR_I 0.1
# define THROTTLE_SONAR_I 0.05
#endif
#ifndef THROTTLE_SONAR_D
# define THROTTLE_SONAR_D 0.06
# define THROTTLE_SONAR_D 0.3 // increasing D by .5
#endif
#ifndef THROTTLE_SONAR_IMAX
# define THROTTLE_SONAR_IMAX 60
# define THROTTLE_SONAR_IMAX 20
#endif

1
ArduCopterMega/setup.pde
View File

@ -409,6 +409,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
if (!strcmp_P(argv[1].str, PSTR("c"))) {
compass.set_offsets(_offsets);
compass.save_offsets();
report_compass();
return (0);
}

2
ArduCopterMega/system.pde
View File

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