From d2ce6ebf8dfc06fbd7a1c48ec067baf3a0bbd32b Mon Sep 17 00:00:00 2001 From: jasonshort Date: Fri, 24 Jun 2011 04:37:54 +0000 Subject: [PATCH] 2.0.26 minor updates, Arming motors now for only Stabilize, Simple, and Acro Modes. V Octo Support git-svn-id: https://arducopter.googlecode.com/svn/trunk@2657 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 1 + ArduCopterMega/ArduCopterMega.pde | 9 +----- ArduCopterMega/Camera.pde | 1 - ArduCopterMega/GCS_Mavlink.pde | 2 +- ArduCopterMega/Log.pde | 6 ++-- ArduCopterMega/Mavlink_Common.h | 15 ++-------- ArduCopterMega/defines.h | 6 ++-- ArduCopterMega/motors.pde | 27 +++++++++-------- ArduCopterMega/motors_octa.pde | 48 +++++++++++++++++++++++++------ ArduCopterMega/setup.pde | 2 +- ArduCopterMega/system.pde | 4 +-- 11 files changed, 70 insertions(+), 51 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 9eb9231ccd..ef9adb4470 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -33,6 +33,7 @@ /* PLUS_FRAME X_FRAME + V_FRAME */ diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index ccd51d107e..9cc40ce5c5 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -45,10 +45,10 @@ version 2.1 of the License, or (at your option) any later version. #include // MAVLink GCS definitions // Configuration +#include "defines.h" #include "config.h" // Local modules -#include "defines.h" #include "Parameters.h" #include "GCS.h" #include "HIL.h" @@ -496,13 +496,6 @@ void loop() //if (delta_ms_fast_loop > 6) // Log_Write_Performance(); - /* - if(delta_ms_fast_loop > 11){ - update_timer_light(true); - //Serial.println(delta_ms_fast_loop,DEC); - }else{ - update_timer_light(false); - }*/ // Execute the fast loop // --------------------- diff --git a/ArduCopterMega/Camera.pde b/ArduCopterMega/Camera.pde index 3937bfb065..4c2cf51911 100644 --- a/ArduCopterMega/Camera.pde +++ b/ArduCopterMega/Camera.pde @@ -9,7 +9,6 @@ void init_camera() g.rc_camera_pitch.radio_trim = 1500; g.rc_camera_pitch.radio_max = 2000; - g.rc_camera_roll.set_angle(4500); g.rc_camera_roll.radio_min = 1000; g.rc_camera_roll.radio_trim = 1500; diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 4d2c32ec9d..a57a291148 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -726,7 +726,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 diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 063a85bf4a..d2c52fc87d 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -579,14 +579,14 @@ void Log_Write_Performance() //* DataFlash.WriteLong( millis()- perf_mon_timer); - DataFlash.WriteInt( mainLoop_count); - DataFlash.WriteInt( G_Dt_max); + DataFlash.WriteInt ( mainLoop_count); + DataFlash.WriteInt ( G_Dt_max); DataFlash.WriteByte( dcm.gyro_sat_count); DataFlash.WriteByte( imu.adc_constraints); DataFlash.WriteByte( dcm.renorm_sqrt_count); DataFlash.WriteByte( dcm.renorm_blowup_count); DataFlash.WriteByte( gps_fix_count); - DataFlash.WriteInt( (int)(dcm.get_health() * 1000)); + DataFlash.WriteInt ( (int)(dcm.get_health() * 1000)); //*/ //PM, 20005, 3742, 10,0,0,0,0,89,1000, diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index 44e0b2ac4f..9dd10d1de2 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -44,18 +44,6 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui uint8_t nav_mode = MAV_NAV_VECTOR; switch(control_mode) { - case ACRO: - mode = MAV_MODE_MANUAL; - break; - case STABILIZE: - mode = MAV_MODE_GUIDED; - break; - case SIMPLE: - mode = MAV_MODE_TEST1; - break; - case ALT_HOLD: - mode = MAV_MODE_TEST2; - break; case LOITER: mode = MAV_MODE_AUTO; nav_mode = MAV_NAV_HOLD; @@ -68,6 +56,9 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui mode = MAV_MODE_AUTO; nav_mode = MAV_NAV_RETURNING; break; + default: + mode = control_mode + 100; + } uint8_t status = MAV_STATE_ACTIVE; diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index db488664fb..a67e7e03ce 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -24,7 +24,7 @@ #define PLUS_FRAME 0 #define X_FRAME 1 - +#define V_FRAME 2 // LED output #define NORMAL_LEDS 0 @@ -106,8 +106,8 @@ // ---------------- #define STABILIZE 0 // hold level position #define ACRO 1 // rate control -#define ALT_HOLD 2 // AUTO control -#define SIMPLE 3 // +#define SIMPLE 2 // +#define ALT_HOLD 3 // AUTO control #define AUTO 4 // AUTO control #define GCS_AUTO 5 // AUTO control #define LOITER 6 // Hold a single location diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index dff767fffb..cf2768620f 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -17,21 +17,24 @@ void arm_motors() // full right if (g.rc_4.control_in > 4000) { - if (arming_counter > AUTO_LEVEL_DELAY){ - auto_level_counter = 255; - arming_counter = 0; + if (control_mode < ALT_HOLD){ - }else if (arming_counter == ARM_DELAY){ - motor_armed = true; - arming_counter = ARM_DELAY; + if (arming_counter > AUTO_LEVEL_DELAY){ + auto_level_counter = 255; + arming_counter = 0; - // Remember Orientation - // --------------------------- - init_simple_bearing(); + }else if (arming_counter == ARM_DELAY){ + motor_armed = true; + arming_counter = ARM_DELAY; - arming_counter++; - } else{ - arming_counter++; + // Remember Orientation + // --------------------------- + init_simple_bearing(); + + arming_counter++; + } else{ + arming_counter++; + } } // full left diff --git a/ArduCopterMega/motors_octa.pde b/ArduCopterMega/motors_octa.pde index 00cecc3416..4aa867df24 100644 --- a/ArduCopterMega/motors_octa.pde +++ b/ArduCopterMega/motors_octa.pde @@ -36,9 +36,9 @@ void output_motors_armed() //Back side motor_out[CH_7] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT - motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT + motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT - }else{ + }if(g.frame_orientation == PLUS_FRAME){ roll_out = (float)g.rc_1.pwm_out * 0.71; pitch_out = (float)g.rc_2.pwm_out * 0.71; @@ -47,16 +47,48 @@ void output_motors_armed() motor_out[CH_2] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT motor_out[CH_11] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT - //Left side - motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT + //Left side + motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT - //Right side - motor_out[CH_3] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT + //Right side + motor_out[CH_3] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT //Back side motor_out[CH_7] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT + + }else{ + + int roll_out2, pitch_out2; + int roll_out3, pitch_out3; + int roll_out4, pitch_out4; + + roll_out = g.rc_1.pwm_out; + pitch_out = g.rc_2.pwm_out; + roll_out2 = (float)g.rc_1.pwm_out * 0.833; + pitch_out2 = (float)g.rc_2.pwm_out * 0.34; + roll_out3 = (float)g.rc_1.pwm_out * 0.666; + pitch_out3 = (float)g.rc_2.pwm_out * 0.32; + roll_out4 = (float)g.rc_1.pwm_out * 0.5; + pitch_out4 = (float)g.rc_2.pwm_out * 0.98; + + //Front side + motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT + motor_out[CH_11] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT + + //Left side + motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT + motor_out[CH_8] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK + + //Right side + motor_out[CH_3] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK + motor_out[CH_2] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT + + //Back side + motor_out[CH_7] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT + motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT + } // Yaw @@ -67,8 +99,8 @@ void output_motors_armed() motor_out[CH_1] -= g.rc_4.pwm_out; // CW motor_out[CH_7] -= g.rc_4.pwm_out; // CW - motor_out[CH_10] -= g.rc_4.pwm_out; // CW - motor_out[CH_3] -= g.rc_4.pwm_out; // CW + motor_out[CH_10] -= g.rc_4.pwm_out; // CW + motor_out[CH_3] -= g.rc_4.pwm_out; // CW // limit output so motors don't stop motor_out[CH_1] = max(motor_out[CH_1], out_min); diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 55c22049bc..d74853ea21 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -653,7 +653,7 @@ setup_gyro(uint8_t argc, const Menu::arg *argv) void clear_offsets() { - Vector3f _offsets; + Vector3f _offsets(0.0,0.0,0.0); compass.set_offsets(_offsets); compass.save_offsets(); } diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index ff8f528aab..be8f5ebde4 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -10,7 +10,7 @@ The init_ardupilot function processes everything we need for an in - air restart extern 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 extern 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 +extern int8_t planner_mode(uint8_t argc, const Menu::arg *argv); // in planner.pde // This is the help function // PSTR is an AVR macro to read strings from flash memory @@ -40,7 +40,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "AC 2.0.25 Beta", main_menu_commands); +MENU(main_menu, "AC 2.0.26 Beta", main_menu_commands); void init_ardupilot() {