From 8f036514f46d4024038fbfeb44fc97fe40ce7df2 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sat, 25 Jun 2011 05:59:06 +0000 Subject: [PATCH] minor formatting, enabled sonar spike filter. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2670 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 87 +++++++------------------------ ArduCopterMega/GCS_Mavlink.pde | 17 ++++-- ArduCopterMega/system.pde | 4 ++ 3 files changed, 37 insertions(+), 71 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 09cbb53efd..3e77b642d5 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -28,6 +28,7 @@ HappyKillmore :Mavlink GCS Micheal Oborne :Mavlink GCS Jack Dunkle :Alpha testing Christof Schmid :Alpha testing +Guntars :Arming safety suggestion And much more so PLEASE PM me on DIYDRONES to add your contribution to the List @@ -507,6 +508,7 @@ void loop() // We want this to execute fast // ---------------------------- if (millis() - fast_loopTimer >= 5) { + //PORTK |= B00010000; delta_ms_fast_loop = millis() - fast_loopTimer; fast_loopTimer = millis(); load = float(fast_loopTimeStamp - fast_loopTimer) / delta_ms_fast_loop; @@ -549,6 +551,7 @@ void loop() } } } + //PORTK &= B11101111; } // Main loop @@ -610,8 +613,7 @@ void medium_loop() #if HIL_MODE != HIL_MODE_ATTITUDE if(g.compass_enabled){ compass.read(); // Read magnetometer - compass.calculate(dcm.get_dcm_matrix()); // Calculate heading - //compass.calculate(dcm.roll, dcm.pitch); // Calculate heading + compass.calculate(dcm.get_dcm_matrix()); // Calculate heading compass.null_offsets(dcm.get_dcm_matrix()); } #endif @@ -701,7 +703,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); @@ -711,14 +712,13 @@ void medium_loop() gcs.send_message(MSG_ATTITUDE); // Sends attitude data #endif - #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.data_stream_send(5,45); - #endif + //#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) + // hil.data_stream_send(5,45); + //#endif if (g.log_bitmask & MASK_LOG_MOTORS) Log_Write_Motors(); - break; // This case controls the slow loop @@ -882,15 +882,15 @@ void slow_loop() gcs.send_message(MSG_CPU_LOAD, load*100); #endif - #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) - hil.data_stream_send(1,5); - #endif + // XXX This was taking 14ms for no reason!!! + //#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) + //hil.data_stream_send(1,5); + //#endif #if CHANNEL_6_TUNING != CH6_NONE tuning(); #endif - // filter out the baro offset. //if(baro_alt_offset > 0) baro_alt_offset--; //if(baro_alt_offset < 0) baro_alt_offset++; @@ -1309,66 +1309,22 @@ void update_alt() baro_alt = read_barometer(); int temp_sonar = sonar.read(); - // spike filter - //if((temp_sonar - sonar_alt) < 50){ - sonar_alt = temp_sonar; - //} - - scale = (sonar_alt - 300) / 300; - scale = constrain(scale, 0, 1); - - current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; - - }else{ - baro_alt = read_barometer(); - // no sonar altitude - current_loc.alt = baro_alt + home.alt; - } - - #endif -} - -// updated at 10hz -void update_alt2() -{ - altitude_sensor = BARO; - - #if HIL_MODE == HIL_MODE_ATTITUDE - current_loc.alt = g_gps->altitude; - #else - - if(g.sonar_enabled){ - // filter out offset - - // read barometer - baro_alt = read_barometer(); - int temp_sonar = sonar.read(); - // spike filter if((temp_sonar - sonar_alt) < 50){ sonar_alt = temp_sonar; } - // decide if we're using sonar - if ((baro_alt < 1200) || sonar_alt < 300){ - // correct alt for angle of the sonar - float temp = cos_pitch_x * cos_roll_x; - temp = max(temp, 0.707); - - sonar_alt = (float)sonar_alt * temp; + /* + doesn't really seem to be a need for this using EZ0: + float temp = cos_pitch_x * cos_roll_x; + temp = max(temp, 0.707); + sonar_alt = (float)sonar_alt * temp; + */ - if(sonar_alt < 450){ - altitude_sensor = SONAR; - baro_alt_offset = sonar_alt - baro_alt; - } - } + scale = (sonar_alt - 300) / 300; + scale = constrain(scale, 0, 1); - // calculate our altitude - if(altitude_sensor == BARO){ - current_loc.alt = baro_alt + baro_alt_offset + home.alt; - }else{ - current_loc.alt = sonar_alt + home.alt; - } + current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; }else{ baro_alt = read_barometer(); @@ -1376,7 +1332,6 @@ void update_alt2() current_loc.alt = baro_alt + home.alt; } - //Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt); #endif } @@ -1407,8 +1362,6 @@ void tuning(){ g.pid_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0); #elif CHANNEL_6_TUNING == CH6_STABLIZE_KD - // uncomment me out to try in flight dampening, 0 = unflyable, .2 = unfun, .13 worked for me. - // use test,radio to get the value to use in your config. g.pid_stabilize_pitch.kD((float)g.rc_6.control_in / 1000.0); g.pid_stabilize_roll.kD((float)g.rc_6.control_in / 1000.0); diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index a57a291148..c7cca094b5 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -22,7 +22,6 @@ streamRatePosition (&_group, 4, 0, PSTR("POSITION")), streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) - { } @@ -172,9 +171,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) int freq = 0; // packet frequency - if (packet.start_stop == 0) freq = 0; // stop sending - else if (packet.start_stop == 1) freq = packet.req_message_rate; // start sending - else break; + if (packet.start_stop == 0) + freq = 0; // stop sending + else if (packet.start_stop == 1) + freq = packet.req_message_rate; // start sending + else + break; switch(packet.req_stream_id){ @@ -188,6 +190,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) streamRateExtra2 = freq; streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. break; + case MAV_DATA_STREAM_RAW_SENSORS: streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly // we will not continue to broadcast raw sensor data at 50Hz. @@ -195,9 +198,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_DATA_STREAM_EXTENDED_STATUS: streamRateExtendedStatus.set_and_save(freq); break; + case MAV_DATA_STREAM_RC_CHANNELS: streamRateRCChannels.set_and_save(freq); break; + case MAV_DATA_STREAM_RAW_CONTROLLER: streamRateRawController.set_and_save(freq); break; @@ -207,15 +212,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_DATA_STREAM_POSITION: streamRatePosition.set_and_save(freq); break; + case MAV_DATA_STREAM_EXTRA1: streamRateExtra1.set_and_save(freq); break; + case MAV_DATA_STREAM_EXTRA2: streamRateExtra2.set_and_save(freq); break; + case MAV_DATA_STREAM_EXTRA3: streamRateExtra3.set_and_save(freq); break; + default: break; } diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index be8f5ebde4..d9c20729bf 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -99,6 +99,10 @@ void init_ardupilot() pinMode(PUSHBUTTON_PIN, INPUT); // unused DDRL |= B00000100; // Set Port L, pin 2 to output for the relay + // XXX set Analog out 14 to output + // 76543210 +// DDRK |= B01010000; + #if MOTOR_LEDS == 1 pinMode(FR_LED, OUTPUT); // GPS status LED pinMode(RE_LED, OUTPUT); // GPS status LED