From c8304114a3624913a716291af775bce3ade50197 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 4 Nov 2011 21:41:51 -0700 Subject: [PATCH] renamed some command variables to align with Arduplane reworked the arming code and moved the DCM gains out. updated climb_rate to include sonar data. --- ArduCopter/APM_Config.h | 2 +- ArduCopter/ArduCopter.pde | 45 +++++-- ArduCopter/Attitude.pde | 2 +- ArduCopter/GCS_Mavlink.pde | 38 +++--- ArduCopter/Log.pde | 2 +- ArduCopter/Parameters.h | 14 +- ArduCopter/commands.pde | 22 ++-- ArduCopter/commands_logic.pde | 4 +- ArduCopter/commands_process.pde | 26 ++-- ArduCopter/control_modes.pde | 2 +- ArduCopter/motors.pde | 225 +++++++++++++++----------------- ArduCopter/setup.pde | 6 +- ArduCopter/system.pde | 14 ++ ArduCopter/test.pde | 30 +++-- ArduPlane/commands_process.pde | 26 ++-- 15 files changed, 239 insertions(+), 219 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 2fbfc1b4da..34ea694279 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -39,7 +39,7 @@ CH7_SAVE_WP */ -#define ACCEL_ALT_HOLD 1 // disabled by default, work in progress +#define ACCEL_ALT_HOLD 0 // disabled by default, work in progress // See the config.h and defines.h files for how to set this up! // diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 18eacd740a..ac612127f5 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -303,7 +303,6 @@ static const float radius_of_earth = 6378100; // meters static const float gravity = 9.81; // meters/ sec^2 static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target -static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate static byte wp_control; // used to control - navgation or loiter static byte command_must_index; // current command memory location @@ -335,9 +334,6 @@ static int airspeed; // m/s * 100 // Location Errors // --------------- -static long altitude_error; // meters * 100 we are off in altitude -static long old_altitude; -static int old_rate; static long yaw_error; // how off are we pointed static long long_error, lat_error; // temp for debugging @@ -361,10 +357,20 @@ static int ground_temperature; // Altitude Sensor variables // ---------------------- -static int sonar_alt; -static int baro_alt; static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR -static int altitude_rate; +static long altitude_error; // meters * 100 we are off in altitude + +static int climb_rate; // m/s * 100 + +static int sonar_alt; +static int old_sonar_alt; +static int sonar_rate; + +static int baro_alt; +static int old_baro_alt; +static int baro_rate; + + // flight mode specific // -------------------- @@ -1206,12 +1212,21 @@ static void update_altitude() return; #else + // calc the vertical accel rate + int temp_alt = (barometer._offset_press - barometer.RawPress) << 1; // invert and scale + baro_rate = (temp_alt - old_baro_alt) * 10; + old_baro_alt = temp_alt; + if(g.sonar_enabled){ // filter out offset float scale; // read barometer - baro_alt = (baro_alt + read_barometer()) >> 1; + baro_alt = (baro_alt + read_barometer()) >> 1; + + // calc rate of change for Sonar + sonar_rate = (sonar_alt - old_sonar_alt) * 10; + old_sonar_alt = sonar_alt; if(baro_alt < 1000){ @@ -1224,21 +1239,23 @@ static void update_altitude() scale = (sonar_alt - 400) / 200; scale = constrain(scale, 0, 1); - current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; + + current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; + climb_rate = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale; + }else{ current_loc.alt = baro_alt + home.alt; + climb_rate = baro_rate; } }else{ + // No Sonar Case baro_alt = read_barometer(); - // no sonar altitude current_loc.alt = baro_alt + home.alt; + climb_rate = baro_rate; } - // calc the vertical accel rate - int temp_rate = (barometer._offset_press - barometer.RawPress) << 1; // invert and scale - altitude_rate = (temp_rate - old_rate) * 10; - old_rate = temp_rate; + #endif } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 2d222dd159..fc117c7b72 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -91,7 +91,7 @@ get_nav_throttle(long z_error) z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85 - rate_error = rate_error - altitude_rate; + rate_error = rate_error - climb_rate; // limit the rate rate_error = constrain(rate_error, -80, 140); diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 1f4a5c10bd..663c10aeac 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -138,23 +138,23 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) const uint8_t rssi = 1; // normalized values scaled to -10000 to 10000 // This is used for HIL. Do not change without discussing with HIL maintainers - - #if FRAME_CONFIG == HELI_FRAME - + + #if FRAME_CONFIG == HELI_FRAME + mavlink_msg_rc_channels_scaled_send( chan, - g.rc_1.servo_out, - g.rc_2.servo_out, - g.rc_3.radio_out, + g.rc_1.servo_out, + g.rc_2.servo_out, + g.rc_3.radio_out, g.rc_4.servo_out, 0, 0, 0, 0, rssi); - + #else - + mavlink_msg_rc_channels_scaled_send( chan, g.rc_1.servo_out, @@ -166,7 +166,7 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) 10000 * g.rc_3.norm_output(), 10000 * g.rc_4.norm_output(), rssi); - + #endif } @@ -273,7 +273,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan) { mavlink_msg_waypoint_current_send( chan, - g.waypoint_index); + g.command_index); } static void NOINLINE send_statustext(mavlink_channel_t chan) @@ -579,7 +579,7 @@ GCS_MAVLINK::update(void) uint32_t tnow = millis(); if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.waypoint_total && + waypoint_request_i <= (unsigned)g.command_total && tnow > waypoint_timelast_request + 500) { waypoint_timelast_request = tnow; send_message(MSG_NEXT_WAYPOINT); @@ -947,7 +947,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_waypoint_count_send( chan,msg->sysid, msg->compid, - g.waypoint_total + 1); // + home + g.command_total + 1); // + home waypoint_timelast_send = millis(); waypoint_sending = true; @@ -974,7 +974,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; // send waypoint - tell_command = get_command_with_index(packet.seq); + tell_command = get_cmd_with_index(packet.seq); // set frame of waypoint uint8_t frame; @@ -990,7 +990,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // time that the mav should loiter in milliseconds uint8_t current = 0; // 1 (true), 0 (false) - if (packet.seq == (uint16_t)g.waypoint_index) + if (packet.seq == (uint16_t)g.command_index) current = 1; uint8_t autocontinue = 1; // 1 (true), 0 (false) @@ -1115,7 +1115,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // clear all waypoints uint8_t type = 0; // ok (0), error(1) - g.waypoint_total.set_and_save(0); + g.command_total.set_and_save(0); // send acknowledgement 3 times to makes sure it is received for (int i=0;i<3;i++) @@ -1136,7 +1136,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // set current command change_command(packet.seq); - mavlink_msg_waypoint_current_send(chan, g.waypoint_index); + mavlink_msg_waypoint_current_send(chan, g.command_index); break; } @@ -1153,7 +1153,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.count > MAX_WAYPOINTS) { packet.count = MAX_WAYPOINTS; } - g.waypoint_total.set_and_save(packet.count - 1); + g.command_total.set_and_save(packet.count - 1); waypoint_timelast_receive = millis(); waypoint_receiving = true; @@ -1308,7 +1308,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) waypoint_timelast_request = 0; waypoint_request_i++; - if (waypoint_request_i > (uint16_t)g.waypoint_total){ + if (waypoint_request_i > (uint16_t)g.command_total){ uint8_t type = 0; // ok (0), error(1) mavlink_msg_waypoint_ack_send( @@ -1643,7 +1643,7 @@ void GCS_MAVLINK::queued_waypoint_send() { if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.waypoint_total) { + waypoint_request_i <= (unsigned)g.command_total) { mavlink_msg_waypoint_request_send( chan, waypoint_dest_sysid, diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 304ee2d653..a21dce6b6b 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -762,7 +762,7 @@ static void Log_Write_Cmd(byte num, struct Location *wp) DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_CMD_MSG); - DataFlash.WriteByte(g.waypoint_total); + DataFlash.WriteByte(g.command_total); DataFlash.WriteByte(num); DataFlash.WriteByte(wp->id); diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 73c5f4cb81..b1fee3418e 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -21,7 +21,7 @@ public: // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) - // GCS will interpret values 0-9 as ArduPilotMega. Developers may use + // GCS will interpret values 0-9 as ArduPilotMega. Developers may use // values within that range to identify different branches. // static const uint16_t k_software_type = 10; // 0 for APM trunk @@ -146,8 +146,8 @@ public: // 220: Waypoint data // k_param_waypoint_mode = 220, - k_param_waypoint_total, - k_param_waypoint_index, + k_param_command_total, + k_param_command_index, k_param_command_must_index, k_param_waypoint_radius, k_param_loiter_radius, @@ -193,8 +193,8 @@ public: // Waypoints // AP_Int8 waypoint_mode; - AP_Int8 waypoint_total; - AP_Int8 waypoint_index; + AP_Int8 command_total; + AP_Int8 command_index; AP_Int8 command_must_index; AP_Int8 waypoint_radius; AP_Int8 loiter_radius; @@ -316,8 +316,8 @@ public: low_voltage (LOW_VOLTAGE, k_param_low_voltage, PSTR("LOW_VOLT")), waypoint_mode (0, k_param_waypoint_mode, PSTR("WP_MODE")), - waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), - waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), + command_total (0, k_param_command_total, PSTR("WP_TOTAL")), + command_index (0, k_param_command_index, PSTR("WP_INDEX")), command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")), waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), loiter_radius (LOITER_RADIUS * 100, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index c9d1660bf6..f8af4f2361 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -3,7 +3,7 @@ static void init_commands() { // zero is home, but we always load the next command (1), in the code. - g.waypoint_index = 0; + g.command_index = 0; // This are registers for the current may and must commands // setting to zero will allow them to be written to by new commands @@ -22,13 +22,13 @@ static void clear_command_queue(){ // Getters // ------- -static struct Location get_command_with_index(int i) +static struct Location get_cmd_with_index(int i) { struct Location temp; // Find out proper location in memory by using the start_byte position + the index // -------------------------------------------------------------------------------- - if (i > g.waypoint_total) { + if (i > g.command_total) { // we do not have a valid command to load // return a WP with a "Blank" id temp.id = CMD_BLANK; @@ -77,7 +77,7 @@ static struct Location get_command_with_index(int i) // ------- static void set_command_with_index(struct Location temp, int i) { - i = constrain(i, 0, g.waypoint_total.get()); + i = constrain(i, 0, g.command_total.get()); uint32_t mem = WP_START_BYTE + (i * WP_SIZE); eeprom_write_byte((uint8_t *) mem, temp.id); @@ -100,18 +100,18 @@ static void set_command_with_index(struct Location temp, int i) static void increment_WP_index() { - if (g.waypoint_index < g.waypoint_total) { - g.waypoint_index++; + if (g.command_index < g.command_total) { + g.command_index++; } - SendDebugln(g.waypoint_index,DEC); + SendDebugln(g.command_index,DEC); } /* static void decrement_WP_index() { - if (g.waypoint_index > 0) { - g.waypoint_index.set_and_save(g.waypoint_index - 1); + if (g.command_index > 0) { + g.command_index.set_and_save(g.command_index - 1); } }*/ @@ -135,7 +135,7 @@ static Location get_LOITER_home_wp() next_WP = current_loc; // read home position - struct Location temp = get_command_with_index(0); // 0 = home + struct Location temp = get_cmd_with_index(0); // 0 = home temp.id = MAV_CMD_NAV_LOITER_UNLIM; temp.alt = read_alt_to_hold(); return temp; @@ -149,7 +149,7 @@ It precalculates all the necessary stuff. static void set_next_WP(struct Location *wp) { //SendDebug("MSG wp_index: "); - //SendDebugln(g.waypoint_index, DEC); + //SendDebugln(g.command_index, DEC); // copy the current WP into the OldWP slot // --------------------------------------- diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 9c8faf8295..f9fc541e10 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -660,7 +660,7 @@ static void do_jump() command_may_index = 0; // set pointer to desired index - g.waypoint_index = next_command.p1 - 1; + g.command_index = next_command.p1 - 1; } else if (jump == 0){ // we're done, move along @@ -668,7 +668,7 @@ static void do_jump() } else if (jump == -1) { // repeat forever - g.waypoint_index = next_command.p1 - 1; + g.command_index = next_command.p1 - 1; } } diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 2e46ac1e4c..3214d215bf 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -2,22 +2,22 @@ // For changing active command mid-mission //---------------------------------------- -static void change_command(uint8_t index) +static void change_command(uint8_t cmd_index) { - struct Location temp = get_command_with_index(index); + struct Location temp = get_cmd_with_index(cmd_index); if (temp.id > MAV_CMD_NAV_LAST ){ gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd")); } else { command_must_index = NO_COMMAND; next_command.id = NO_COMMAND; - g.waypoint_index = index - 1; + g.command_index = cmd_index - 1; update_commands(); } } -// called by 10 Hz Medium loop -// --------------------------- +// called by 10 Hz loop +// -------------------- static void update_commands(void) { // fill command queue with a new command if available @@ -25,10 +25,10 @@ static void update_commands(void) // fetch next command if the next command queue is empty // ----------------------------------------------------- - if (g.waypoint_index < g.waypoint_total) { + if (g.command_index < g.command_total) { // only if we have a cmd stored in EEPROM - next_command = get_command_with_index(g.waypoint_index + 1); + next_command = get_cmd_with_index(g.command_index + 1); //Serial.printf("queue CMD %d\n", next_command.id); } } @@ -50,7 +50,7 @@ static void update_commands(void) // We acted on the queue - let's debug that // ---------------------------------------- - print_wp(&next_command, g.waypoint_index); + print_wp(&next_command, g.command_index); // invalidate command queue so a new one is loaded // ----------------------------------------------- @@ -88,11 +88,11 @@ process_next_command() if (next_command.id < MAV_CMD_NAV_LAST ){ // we remember the index of our mission here - command_must_index = g.waypoint_index + 1; + command_must_index = g.command_index + 1; // Save CMD to Log if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index + 1, &next_command); + Log_Write_Cmd(g.command_index + 1, &next_command); // Act on the new command process_must(); @@ -106,7 +106,7 @@ process_next_command() if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ // we remember the index of our mission here - command_may_index = g.waypoint_index + 1; + command_may_index = g.command_index + 1; //SendDebug("MSG new may "); //SendDebugln(next_command.id,DEC); @@ -115,7 +115,7 @@ process_next_command() // Save CMD to Log if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index + 1, &next_command); + Log_Write_Cmd(g.command_index + 1, &next_command); process_may(); return true; @@ -128,7 +128,7 @@ process_next_command() //SendDebugln(next_command.id,DEC); if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index + 1, &next_command); + Log_Write_Cmd(g.command_index + 1, &next_command); process_now(); return true; } diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 632d69c862..e40a10dd00 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -99,7 +99,7 @@ static void read_trim_switch() // set the next_WP CH7_wp_index++; current_loc.id = MAV_CMD_NAV_WAYPOINT; - g.waypoint_total.set_and_save(CH7_wp_index); + g.command_total.set_and_save(CH7_wp_index); set_command_with_index(current_loc, CH7_wp_index); } } diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 0005c98148..5ff9bcb62a 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -1,9 +1,9 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define ARM_DELAY 10 // one second -#define DISARM_DELAY 10 // one second -#define LEVEL_DELAY 70 // twelve seconds -#define AUTO_LEVEL_DELAY 90 // twentyfive seconds +// 10 = 1 second +#define ARM_DELAY 30 +#define DISARM_DELAY 20 +#define LEVEL_DELAY 100 // called at 10hz @@ -11,133 +11,114 @@ static void arm_motors() { static int arming_counter; - // Arm motor output : Throttle down and full yaw right for more than 2 seconds - if (g.rc_3.control_in == 0){ + // don't allow arming/disarming in anything but manual + if ((g.rc_3.control_in > 0) || (control_mode >= ALT_HOLD) || (arming_counter > LEVEL_DELAY)){ + arming_counter = 0; + return; + } - // full right - if (g.rc_4.control_in > 4000) { - - // don't allow arming in anything but manual - if (control_mode < ALT_HOLD){ - - if (arming_counter > AUTO_LEVEL_DELAY){ - auto_level_counter = 155; - arming_counter = 0; - - }else if (arming_counter == ARM_DELAY){ - #if HIL_MODE != HIL_MODE_DISABLED - gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS")); - #endif - motor_armed = true; - arming_counter = ARM_DELAY; - - #if PIEZO_ARMING == 1 - piezo_beep(); - piezo_beep(); - #endif - - // Tune down DCM - // ------------------- - #if HIL_MODE != HIL_MODE_ATTITUDE - dcm.kp_roll_pitch(0.030000); - dcm.ki_roll_pitch(0.00001278), // 50 hz I term - //dcm.ki_roll_pitch(0.000006); - #endif - - // tune down compass - // ----------------- - dcm.kp_yaw(0.08); - dcm.ki_yaw(0); - - // Remember Orientation - // -------------------- - init_simple_bearing(); - - // init the Z damopener - // -------------------- - #if ACCEL_ALT_HOLD == 1 - init_z_damper(); - #endif - - // Reset home position - // ---------------------- - if(home_is_set) - init_home(); - - if(did_ground_start == false){ - did_ground_start = true; - startup_ground(); - } - - #if HIL_MODE != HIL_MODE_ATTITUDE - // read Baro pressure at ground - - // this resets Baro for more accuracy - //----------------------------------- - init_barometer(); - #endif - - // temp hack - motor_light = true; - digitalWrite(A_LED_PIN, HIGH); - - arming_counter++; - } else{ - arming_counter++; - } - } - - // full left - }else if (g.rc_4.control_in < -4000) { - //Serial.print(arming_counter, DEC); - if (arming_counter > LEVEL_DELAY){ - //Serial.print("init"); - imu.init_accel(mavlink_delay); - arming_counter = 0; - - }else if (arming_counter == DISARM_DELAY){ - #if HIL_MODE != HIL_MODE_DISABLED - gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); - #endif - - motor_armed = false; - arming_counter = DISARM_DELAY; - compass.save_offsets(); - - #if PIEZO_ARMING == 1 - piezo_beep(); - #endif - - // Tune down DCM - // ------------------- - #if HIL_MODE != HIL_MODE_ATTITUDE - //dcm.kp_roll_pitch(0.12); // higher for fast recovery - //dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop - #endif - - // tune up compass - // ----------------- - dcm.kp_yaw(0.8); - dcm.ki_yaw(0.00004); - - // Clear throttle slew - // ------------------- - //throttle_slew = 0; - - arming_counter++; - - }else{ - arming_counter++; - } - // centered - }else{ + // full right + if (g.rc_4.control_in > 4000) { + if (arming_counter == LEVEL_DELAY){ + Serial.printf("\nAL\n"); + // begin auto leveling + auto_level_counter = 200; arming_counter = 0; + + }else if (arming_counter == ARM_DELAY){ + if(motor_armed == false){ + // arm the motors and configure for flight + init_arm_motors(); + } + // keep going up + arming_counter++; + } else{ + arming_counter++; } + + // full left + }else if (g.rc_4.control_in < -4000) { + if (arming_counter == LEVEL_DELAY){ + Serial.printf("\nLEV\n"); + + // begin manual leveling + imu.init_accel(mavlink_delay); + arming_counter = 0; + + }else if (arming_counter == DISARM_DELAY){ + if(motor_armed == true){ + // arm the motors and configure for flight + init_disarm_motors(); + } + // keep going up + arming_counter++; + }else{ + arming_counter++; + } + + // Yaw is centered }else{ arming_counter = 0; } } +static void init_arm_motors() +{ + Serial.printf("\nARM\n"); + #if HIL_MODE != HIL_MODE_DISABLED + gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS")); + #endif + + motor_armed = true; + + #if PIEZO_ARMING == 1 + piezo_beep(); + piezo_beep(); + #endif + + // Remember Orientation + // -------------------- + init_simple_bearing(); + + // Reset home position + // ---------------------- + if(home_is_set) + init_home(); + + if(did_ground_start == false){ + did_ground_start = true; + startup_ground(); + } + + #if HIL_MODE != HIL_MODE_ATTITUDE + // read Baro pressure at ground - + // this resets Baro for more accuracy + //----------------------------------- + init_barometer(); + #endif + + // temp hack + motor_light = true; + digitalWrite(A_LED_PIN, HIGH); +} + + +static void init_disarm_motors() +{ + Serial.printf("\nDISARM\n"); + #if HIL_MODE != HIL_MODE_DISABLED + gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); + #endif + + motor_armed = false; + compass.save_offsets(); + + #if PIEZO_ARMING == 1 + piezo_beep(); + #endif +} + /***************************************** * Set the flight control servos based on the current calculated values *****************************************/ diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index db5c615eb1..937ea16daa 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -815,12 +815,12 @@ static void report_batt_monitor() static void report_wp(byte index = 255) { if(index == 255){ - for(byte i = 0; i <= g.waypoint_total; i++){ - struct Location temp = get_command_with_index(i); + for(byte i = 0; i <= g.command_total; i++){ + struct Location temp = get_cmd_with_index(i); print_wp(&temp, i); } }else{ - struct Location temp = get_command_with_index(index); + struct Location temp = get_cmd_with_index(index); print_wp(&temp, index); } } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index c4b6573378..8c4e333f45 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -289,6 +289,20 @@ static void init_ardupilot() // --------------------------- reset_control_switch(); + #if HIL_MODE != HIL_MODE_ATTITUDE + dcm.kp_roll_pitch(0.030000); + dcm.ki_roll_pitch(0.00001278), // 50 hz I term + dcm.kp_yaw(0.08); + dcm.ki_yaw(0.00004); + #endif + + // init the Z damopener + // -------------------- + #if ACCEL_ALT_HOLD == 1 + init_z_damper(); + #endif + + startup_ground(); Log_Write_Startup(); diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 9311475fbe..cb49360f8f 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -6,7 +6,7 @@ // are defined below. Order matters to the compiler. static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); static int8_t test_radio(uint8_t argc, const Menu::arg *argv); -static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); +//static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); //static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv); static int8_t test_gps(uint8_t argc, const Menu::arg *argv); static int8_t test_tri(uint8_t argc, const Menu::arg *argv); @@ -56,7 +56,7 @@ static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); const struct Menu::command test_menu_commands[] PROGMEM = { {"pwm", test_radio_pwm}, {"radio", test_radio}, - {"failsafe", test_failsafe}, +// {"failsafe", test_failsafe}, // {"stabilize", test_stabilize}, {"gps", test_gps}, #if HIL_MODE != HIL_MODE_ATTITUDE @@ -246,6 +246,7 @@ test_radio(uint8_t argc, const Menu::arg *argv) } } +/* static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv) { @@ -299,6 +300,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv) return (0); #endif } +*/ /*static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv) @@ -429,7 +431,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) while(1){ //delay(20); - if (millis() - fast_loopTimer >=5) { + if (millis() - fast_loopTimer >=20) { // IMU // --- @@ -447,7 +449,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) update_trig(); } - if(medium_loopCounter == 20){ + if(medium_loopCounter == 1){ //read_radio(); medium_loopCounter = 0; //tuning(); @@ -461,10 +463,13 @@ test_imu(uint8_t argc, const Menu::arg *argv) dcm.kp_roll_pitch(), (float)g.rc_6.control_in / 2000.0); */ - Serial.printf_P(PSTR("%ld, %ld, %ld\n"), + Serial.printf_P(PSTR("%ld, %ld, %ld, | %ld, %ld, %ld\n"), dcm.roll_sensor, dcm.pitch_sensor, - dcm.yaw_sensor); + dcm.yaw_sensor, + (long)(degrees(omega.x) * 100.0), + (long)(degrees(omega.y) * 100.0), + (long)(degrees(omega.z) * 100.0)); if(g.compass_enabled){ compass.read(); // Read magnetometer @@ -740,7 +745,7 @@ test_wp(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100); } - Serial.printf_P(PSTR("%d wp\n"), (int)g.waypoint_total); + Serial.printf_P(PSTR("%d wp\n"), (int)g.command_total); Serial.printf_P(PSTR("Hit rad: %d\n"), (int)g.waypoint_radius); //Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); @@ -800,11 +805,12 @@ test_baro(uint8_t argc, const Menu::arg *argv) delay(100); baro_alt = read_barometer(); - int temp_rate = (barometer._offset_press - barometer.RawPress) << 1; - altitude_rate = (temp_rate - old_rate) * 10; - old_rate = temp_rate; + int temp_alt = (barometer._offset_press - barometer.RawPress) << 1; // invert and scale + baro_rate = (temp_alt - old_baro_alt) * 10; + old_baro_alt = temp_alt; + // 1 2 3 4 5 1 2 3 4 5 - Serial.printf_P(PSTR("Baro: %dcm, rate:%d, %ld, %ld, %d\n"), baro_alt, altitude_rate, barometer.RawTemp, barometer.RawPress, temp_rate); + Serial.printf_P(PSTR("Baro: %dcm, rate:%d, %ld, %ld, %d\n"), baro_alt, climb_rate, barometer.RawTemp, barometer.RawPress, temp_alt); //Serial.printf_P(PSTR("Baro, %d, %ld, %ld, %ld, %ld\n"), baro_alt, barometer.RawTemp, barometer.RawTemp2, barometer.RawPress, barometer.RawPress2); if(Serial.available() > 0){ return (0); @@ -995,7 +1001,7 @@ test_mission(uint8_t argc, const Menu::arg *argv) } g.RTL_altitude.set_and_save(300); - g.waypoint_total.set_and_save(4); + g.command_total.set_and_save(4); g.waypoint_radius.set_and_save(3); test_wp(NULL, NULL); diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index a5dcffcc82..e5a0bcb8ae 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -3,21 +3,23 @@ // For changing active command mid-mission //---------------------------------------- static void change_command(uint8_t cmd_index) -{ +{ struct Location temp = get_cmd_with_index(cmd_index); + if (temp.id > MAV_CMD_NAV_LAST ){ gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); } else { gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index); - nav_command_ID = NO_COMMAND; + + nav_command_ID = NO_COMMAND; next_nav_command.id = NO_COMMAND; - non_nav_command_ID = NO_COMMAND; - nav_command_index = cmd_index - 1; + non_nav_command_ID = NO_COMMAND; + + nav_command_index = cmd_index - 1; g.command_index.set_and_save(cmd_index - 1); process_next_command(); } } - // called by 10 Hz loop // -------------------- @@ -48,10 +50,10 @@ static void process_next_command() { // This function makes sure that we always have a current navigation command // and loads conditional or immediate commands if applicable - + struct Location temp; byte old_index = 0; - + // these are Navigation/Must commands // --------------------------------- if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded @@ -71,7 +73,7 @@ static void process_next_command() nav_command_ID = next_nav_command.id; non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded non_nav_command_ID = NO_COMMAND; - + if (g.log_bitmask & MASK_LOG_CMD) { Log_Write_Cmd(g.command_index, &next_nav_command); } @@ -87,7 +89,7 @@ static void process_next_command() } else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command non_nav_command_index++; } - + //gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index); //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index); //gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID); @@ -106,8 +108,8 @@ static void process_next_command() if (g.log_bitmask & MASK_LOG_CMD) { Log_Write_Cmd(g.command_index, &next_nonnav_command); } - - process_non_nav_command(); + + process_non_nav_command(); } } @@ -134,7 +136,7 @@ static void process_non_nav_command() if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) { handle_process_condition_command(); - } else { + } else { handle_process_do_command(); // flag command ID so a new one is loaded // -----------------------------------------