From a7a4f999e36233a70b969fc0e9183a16ba9edd57 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sat, 2 Jul 2011 22:44:59 +0000 Subject: [PATCH] AC 2.0.32 Beta new Loiter PIDs tuned down control on standard frame shunk the mixing range for sonar from 3 meters to 2 meters added framework for Guided mode - not tested! removed pitch and roll deadzone decoupled SIMPLE mode from LOITER PIDs Synced Mavlink with APM Logs report correct WP number now. disabled Sonar spike filter. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2722 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 1 + ArduCopterMega/ArduCopterMega.pde | 38 ++-- ArduCopterMega/GCS_Mavlink.pde | 267 +++++++++++++++++++++------- ArduCopterMega/Log.pde | 3 +- ArduCopterMega/Mavlink_Common.h | 21 ++- ArduCopterMega/commands.pde | 12 +- ArduCopterMega/commands_process.pde | 6 +- ArduCopterMega/config.h | 14 +- ArduCopterMega/control_modes.pde | 6 + ArduCopterMega/defines.h | 2 +- ArduCopterMega/navigation.pde | 15 +- ArduCopterMega/radio.pde | 5 +- ArduCopterMega/read_me.text | 6 +- ArduCopterMega/system.pde | 24 ++- 14 files changed, 294 insertions(+), 126 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 784b30f48f..6e3f297e83 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -5,6 +5,7 @@ //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD +#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode) #define FRAME_CONFIG QUAD_FRAME /* diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 8723c83f36..20cbf06655 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -227,10 +227,6 @@ GPS *g_gps; //////////////////////////////////////////////////////////////////////////////// // Global variables //////////////////////////////////////////////////////////////////////////////// - -byte control_mode = STABILIZE; -byte oldSwitchPosition; // for remembering the control mode switch - const char *comma = ","; const char* flight_mode_strings[] = { @@ -239,7 +235,7 @@ const char* flight_mode_strings[] = { "SIMPLE", "ALT_HOLD", "AUTO", - "GCS_AUTO", + "GUIDED", "LOITER", "RTL"}; @@ -257,8 +253,9 @@ const char* flight_mode_strings[] = { // Radio // ----- -int motor_out[8]; -Vector3f omega; +byte control_mode = STABILIZE; +byte oldSwitchPosition; // for remembering the control mode switch +int motor_out[8]; // Heli // ---- @@ -280,6 +277,7 @@ boolean motor_auto_armed; // if true, boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold byte yaw_debug; bool did_clear_yaw_control; +Vector3f omega; // LED output // ---------- @@ -335,7 +333,7 @@ float crosstrack_error; // meters we are off trackline long distance_error; // distance to the WP long yaw_error; // how off are we pointed long long_error, lat_error; // temp for debugging - +int loiter_error_max; // Battery Sensors // --------------- float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter @@ -445,6 +443,7 @@ struct Location next_WP; // next waypoint struct Location target_WP; // where do we want to you towards? struct Location simple_WP; // struct Location next_command; // command preloaded +struct Location guided_WP; // guided mode waypoint long target_altitude; // used for boolean home_is_set; // Flag for if we have g_gps lock and have set the home location @@ -1094,14 +1093,17 @@ void update_current_flight_mode(void) simple_WP.lat = 0; simple_WP.lng = 0; - next_WP.lng = (float)g.rc_1.control_in *.4; // X: 4500 / 2 = 2250 = 25 meteres - next_WP.lat = -((float)g.rc_2.control_in *.4); // Y: 4500 / 2 = 2250 = 25 meteres + next_WP.lng = (float)g.rc_1.control_in * .9; // X: 4500 * .7 = 2250 = 25 meteres + next_WP.lat = -(float)g.rc_2.control_in * .9; // Y: 4500 * .7 = 2250 = 25 meteres + //next_WP.lng = g.rc_1.control_in; // X: 4500 * .7 = 2250 = 25 meteres + //next_WP.lat = -g.rc_2.control_in; // Y: 4500 * .7 = 2250 = 25 meteres // calc a new bearing nav_bearing = get_bearing(&simple_WP, &next_WP) + initial_simple_bearing; nav_bearing = wrap_360(nav_bearing); wp_distance = get_distance(&simple_WP, &next_WP); calc_bearing_error(); + /* Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ", next_WP.lat, @@ -1179,6 +1181,7 @@ void update_current_flight_mode(void) output_stabilize_pitch(); break; + case GUIDED: case RTL: // Output Pitch, Roll, Yaw and Throttle // ------------------------------------ @@ -1252,6 +1255,10 @@ void update_navigation() case RTL: // calculates desired Yaw update_nav_yaw(); + + case GUIDED: + update_nav_yaw(); + // switch passthrough to LOITER case LOITER: // are we Traversing or Loitering? wp_control = (wp_distance < 50) ? LOITER_MODE : WP_MODE; @@ -1325,9 +1332,11 @@ void update_alt() int temp_sonar = sonar.read(); // spike filter - if((temp_sonar - sonar_alt) < 50){ - sonar_alt = temp_sonar; - } + //if((temp_sonar - sonar_alt) < 50){ + // sonar_alt = temp_sonar; + //} + + sonar_alt = temp_sonar; /* doesn't really seem to be a need for this using EZ0: @@ -1337,7 +1346,7 @@ void update_alt() */ if(baro_alt < 800){ - scale = (sonar_alt - 300) / 300; + 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; @@ -1430,6 +1439,7 @@ void tuning(){ void update_nav_wp() { + // XXX Guided mode!!! if(wp_control == LOITER_MODE){ // calc a pitch to the target calc_loiter_nav(); diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index ad63cc6838..8e25172b5b 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -39,16 +39,6 @@ GCS_MAVLINK::init(BetterStream * port) chan = MAVLINK_COMM_1; } _queued_parameter = NULL; - - // temporary - streamRateRawSensors.set_and_save(0); - streamRateExtendedStatus.set_and_save(0); - streamRateRCChannels.set_and_save(0); - streamRateRawController.set_and_save(0); - streamRatePosition.set_and_save(0); - streamRateExtra1.set_and_save(0); - streamRateExtra2.set_and_save(0); - streamRateExtra3.set_and_save(0); } void @@ -88,18 +78,6 @@ GCS_MAVLINK::update(void) } } -/* -mav2 1 -mav3 3 -mav5 3 -mav6 10 -mav7 10 -mav6 10 -mav7 10 -mav6 10 -mav7 10 -*/ - void GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) { @@ -186,7 +164,9 @@ GCS_MAVLINK::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { - struct Location tell_command; // command for telemetry + struct Location tell_command = {}; // command for telemetry + static uint8_t mav_nav = 255; // For setting mode (some require receipt of 2 messages...) + switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: @@ -195,7 +175,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_request_data_stream_t packet; mavlink_msg_request_data_stream_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) + if (mavlink_check_target(packet.target_system, packet.target_component)) break; int freq = 0; // packet frequency @@ -219,7 +199,6 @@ 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. streamRateExtra3 = freq; // Don't save!! - break; case MAV_DATA_STREAM_RAW_SENSORS: @@ -238,9 +217,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_DATA_STREAM_RAW_CONTROLLER: streamRateRawController = freq; break; - //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: - // streamRateRawSensorFusion.set_and_save(freq); - // break; + + //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: + // streamRateRawSensorFusion.set_and_save(freq); + // break; + case MAV_DATA_STREAM_POSITION: streamRatePosition = freq; break; @@ -251,7 +232,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_DATA_STREAM_EXTRA2: streamRateExtra2 = freq; - //streamRateExtra2.set_and_save(freq); break; case MAV_DATA_STREAM_EXTRA3: @@ -339,10 +319,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_ACTION_CALIBRATE_MAG: case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_PRESSURE: - case MAV_ACTION_REBOOT: // this is a rough interpretation + //case MAV_ACTION_REBOOT: // this is a rough interpretation //startup_IMU_ground(); //result=1; - break; + // break; /* For future implemtation case MAV_ACTION_REC_START: break; @@ -384,6 +364,45 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } + case MAVLINK_MSG_ID_SET_MODE: + { + // decode + mavlink_set_mode_t packet; + mavlink_msg_set_mode_decode(msg, &packet); + + switch(packet.mode){ + + case MAV_MODE_MANUAL: + set_mode(STABILIZE); + break; + + case MAV_MODE_GUIDED: + set_mode(GUIDED); + break; + + case MAV_MODE_AUTO: + if(mav_nav == 255 || mav_nav == MAV_NAV_WAYPOINT) set_mode(AUTO); + if(mav_nav == MAV_NAV_RETURNING) set_mode(RTL); + if(mav_nav == MAV_NAV_LOITER) set_mode(LOITER); + mav_nav = 255; + break; + + case MAV_MODE_TEST1: + set_mode(STABILIZE); + break; + } + } + + /*case MAVLINK_MSG_ID_SET_NAV_MODE: + { + // decode + mavlink_set_nav_mode_t packet; + mavlink_msg_set_nav_mode_decode(msg, &packet); + // To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message + mav_nav = packet.nav_mode; + break; + } + */ case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: { //send_text_P(SEVERITY_LOW,PSTR("waypoint request list")); @@ -391,7 +410,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_waypoint_request_list_t packet; mavlink_msg_waypoint_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) + if (mavlink_check_target(packet.target_system, packet.target_component)) break; // Start sending waypoints @@ -415,14 +434,14 @@ 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; + // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW // decode mavlink_waypoint_request_t packet; mavlink_msg_waypoint_request_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) + if (mavlink_check_target(packet.target_system, packet.target_component)) break; // send waypoint @@ -442,7 +461,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 == g.waypoint_index) + if (packet.seq == (uint16_t)g.waypoint_index) current = 1; uint8_t autocontinue = 1; // 1 (true), 0 (false) @@ -471,7 +490,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_NAV_LOITER_TIME: - param1 = tell_command.p1; // APM loiter time is in ten second increments + param1 = tell_command.p1; // ACM loiter time is in 1 second increments break; case MAV_CMD_CONDITION_DELAY: @@ -534,9 +553,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_waypoint_ack_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; - // check for error - uint8_t type = packet.type; // ok (0), error(1) - // turn off waypoint send waypoint_sending = false; break; @@ -621,20 +637,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // XXX receive a WP from GCS and store in EEPROM case MAVLINK_MSG_ID_WAYPOINT: { - // Check if receiving waypiont - if (!waypoint_receiving) break; - // decode mavlink_waypoint_t packet; mavlink_msg_waypoint_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) break; - // check if this is the requested waypoint - if (packet.seq != waypoint_request_i) break; - - // store waypoint - uint8_t loadAction = 0; // 0 insert in list, 1 exec now - // defaults tell_command.id = packet.command; @@ -728,13 +735,39 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - set_command_with_index(tell_command, packet.seq); + if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission + guided_WP = tell_command; + + // add home alt if needed + if (guided_WP.options & WP_OPTION_ALT_RELATIVE){ + guided_WP.alt += home.alt; + } + + set_mode(GUIDED); + + // make any new wp uploaded instant (in case we are already in Guided mode) + set_next_WP(&guided_WP); + + // verify we recevied the command + mavlink_msg_waypoint_ack_send( + chan, + msg->sysid, + msg->compid, + 0); + + } else { + // Check if receiving waypoints (mission upload expected) + if (!waypoint_receiving) break; + + // check if this is the requested waypoint + if (packet.seq != waypoint_request_i) break; + set_command_with_index(tell_command, packet.seq); // update waypoint receiving state machine waypoint_timelast_receive = millis(); waypoint_request_i++; - if (waypoint_request_i > g.waypoint_total){ + if (waypoint_request_i > (uint16_t)g.waypoint_total){ uint8_t type = 0; // ok (0), error(1) mavlink_msg_waypoint_ack_send( @@ -747,6 +780,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) waypoint_receiving = false; // XXX ignores waypoint radius for individual waypoints, can // only set WP_RADIUS parameter + } } break; } @@ -759,8 +793,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // decode mavlink_param_set_t packet; mavlink_msg_param_set_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; // set parameter @@ -777,7 +812,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // add a small amount before casting parameter values // from float to integer to avoid truncating to the // next lower integer value. - const float rounding_addition = 0.01; + float rounding_addition = 0.01; // fetch the variable type ID var_type = vp->meta_type_id(); @@ -790,13 +825,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) ((AP_Float16 *)vp)->set_and_save(packet.param_value); } else if (var_type == AP_Var::k_typeid_int32) { - ((AP_Int32 *)vp)->set_and_save(packet.param_value + rounding_addition); - + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); } else if (var_type == AP_Var::k_typeid_int16) { - ((AP_Int16 *)vp)->set_and_save(packet.param_value + rounding_addition); - + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); } else if (var_type == AP_Var::k_typeid_int8) { - ((AP_Int8 *)vp)->set_and_save(packet.param_value + rounding_addition); + if (packet.param_value < 0) rounding_addition = -rounding_addition; + ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); } else { // we don't support mavlink set on this parameter break; @@ -816,16 +852,20 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } // end case - -#if ALLOW_RC_OVERRIDE == ENABLED - case MAVLINK_MSG_ID_RC_CHANNELS_RAW: +/* + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. - mavlink_rc_channels_raw_t packet; + if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs + mavlink_rc_channels_override_t packet; int16_t v[8]; - mavlink_msg_rc_channels_raw_decode(msg, &packet); + mavlink_msg_rc_channels_override_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system,packet.target_component)) + break; + v[0] = packet.chan1_raw; v[1] = packet.chan2_raw; v[2] = packet.chan3_raw; @@ -834,12 +874,109 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) v[5] = packet.chan6_raw; v[6] = packet.chan7_raw; v[7] = packet.chan8_raw; - APM_RC.setHIL(v); + rc_override_active = APM_RC.setHIL(v); + rc_override_fs_timer = millis(); + break; + } + + case MAVLINK_MSG_ID_HEARTBEAT: + { + // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes + if(msg->sysid != g.sysid_my_gcs) break; + rc_override_fs_timer = millis(); + //pmTest1++; + break; + } + + #if HIL_MODE != HIL_MODE_DISABLED + // This is used both as a sensor and to pass the location + // in HIL_ATTITUDE mode. + case MAVLINK_MSG_ID_GPS_RAW: + { + // decode + mavlink_gps_raw_t packet; + mavlink_msg_gps_raw_decode(msg, &packet); + + // set gps hil sensor + g_gps->setHIL(packet.usec/1000.0,packet.lat,packet.lon,packet.alt, + packet.v,packet.hdg,0,0); + break; + } + + // Is this resolved? - MAVLink protocol change..... + case MAVLINK_MSG_ID_VFR_HUD: + { + // decode + mavlink_vfr_hud_t packet; + mavlink_msg_vfr_hud_decode(msg, &packet); + + // set airspeed + airspeed = 100*packet.airspeed; + break; + } + +#endif +#if HIL_MODE == HIL_MODE_ATTITUDE + case MAVLINK_MSG_ID_ATTITUDE: + { + // decode + mavlink_attitude_t packet; + mavlink_msg_attitude_decode(msg, &packet); + + // set dcm hil sensor + dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + packet.pitchspeed,packet.yawspeed); break; } #endif +#if HIL_MODE == HIL_MODE_SENSORS + case MAVLINK_MSG_ID_RAW_IMU: + { + // decode + mavlink_raw_imu_t packet; + mavlink_msg_raw_imu_decode(msg, &packet); + // set imu hil sensors + // TODO: check scaling for temp/absPress + float temp = 70; + float absPress = 1; + // Serial.printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc); + // Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro); + + // rad/sec + Vector3f gyros; + gyros.x = (float)packet.xgyro / 1000.0; + gyros.y = (float)packet.ygyro / 1000.0; + gyros.z = (float)packet.zgyro / 1000.0; + // m/s/s + Vector3f accels; + accels.x = (float)packet.xacc / 1000.0; + accels.y = (float)packet.yacc / 1000.0; + accels.z = (float)packet.zacc / 1000.0; + + imu.set_gyro(gyros); + + imu.set_accel(accels); + + compass.setHIL(packet.xmag,packet.ymag,packet.zmag); + break; + } + + case MAVLINK_MSG_ID_RAW_PRESSURE: + { + // decode + mavlink_raw_pressure_t packet; + mavlink_msg_raw_pressure_decode(msg, &packet); + + // set pressure hil sensor + // TODO: check scaling + float temp = 70; + barometer.setHIL(temp,packet.press_diff1); + break; + } +#endif // HIL_MODE +*/ } // end switch } // end handle mavlink diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index d2c52fc87d..d994378c6c 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -73,7 +73,7 @@ print_log_menu(void) Serial.println(); if (last_log_num == 0) { - Serial.printf_P(PSTR("\nNo logs\n")); + Serial.printf_P(PSTR("\nNo logs\nType 'dump 0'.\n\n")); }else{ Serial.printf_P(PSTR("\n%d logs\n"), last_log_num); @@ -107,6 +107,7 @@ dump_log(uint8_t argc, const Menu::arg *argv) if (/*(argc != 2) || */ (dump_log < 1) || (dump_log > last_log_num)) { Serial.printf_P(PSTR("bad log # %d\n"), dump_log); Log_Read(1, 4095); + erase_logs(NULL, NULL); return(-1); } diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index 9dd10d1de2..4a41929bee 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -56,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; + case GUIDED: + mode = MAV_MODE_GUIDED; + break; default: mode = control_mode + 100; @@ -63,7 +66,6 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui uint8_t status = MAV_STATE_ACTIVE; uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 - uint8_t motor_block = false; mavlink_msg_sys_status_send( chan, @@ -199,9 +201,9 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui motor_out[2], motor_out[3], motor_out[4], - g.rc_5.radio_out, - g.rc_6.radio_out, - motor_out[7]); + motor_out[7], + motor_out[8], + 0); break; } @@ -238,13 +240,14 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui compass.mag_y, compass.mag_z); - mavlink_msg_raw_pressure_send( + /* This message is not working. Why? + mavlink_msg_scaled_pressure_send( chan, timeStamp, - adc.Ch(AIRSPEED_CH), - barometer.RawPress, - 0, - 0); + (float)barometer.Press/100., + (float)adc.Ch(AIRSPEED_CH), // We don't calculate the differential pressure value anywhere, so use raw + (int)(barometer.Temp*100)); + */ break; } #endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index a866b00c2b..fc0cb460af 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -164,8 +164,8 @@ It precalculates all the necessary stuff. void set_next_WP(struct Location *wp) { - SendDebug("MSG wp_index: "); - SendDebugln(g.waypoint_index, DEC); + //SendDebug("MSG wp_index: "); + //SendDebugln(g.waypoint_index, DEC); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); // copy the current WP into the OldWP slot @@ -180,11 +180,6 @@ void set_next_WP(struct Location *wp) // ----------------------------------------------------------------- target_altitude = current_loc.alt; - // zero out our loiter vals to watch for missed waypoints - //loiter_delta = 0; - //loiter_sum = 0; - //loiter_total = 0; - // this is used to offset the shrinking longitude as we go towards the poles float rads = (abs(next_WP.lat)/t7) * 0.0174532925; scaleLongDown = cos(rads); @@ -192,8 +187,6 @@ void set_next_WP(struct Location *wp) // this is handy for the groundstation wp_totalDistance = get_distance(¤t_loc, &next_WP); - - // this makes the data not log for a GPS read wp_distance = wp_totalDistance; target_bearing = get_bearing(¤t_loc, &next_WP); nav_bearing = target_bearing; @@ -209,6 +202,7 @@ void set_next_WP(struct Location *wp) gcs.print_current_waypoints(); } + // run this at setup on the ground // ------------------------------- void init_home() diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 116bb68914..0913e74d3c 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -94,7 +94,7 @@ process_next_command() // Save CMD to Log if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); + Log_Write_Cmd(g.waypoint_index + 1, &next_command); // Act on the new command process_must(); @@ -117,7 +117,7 @@ process_next_command() // Save CMD to Log if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); + Log_Write_Cmd(g.waypoint_index + 1, &next_command); process_may(); return true; @@ -130,7 +130,7 @@ process_next_command() //SendDebugln(next_command.id,DEC); if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); + Log_Write_Cmd(g.waypoint_index + 1, &next_command); process_now(); return true; } diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index f0da9ea954..a3dc1d46a1 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -329,26 +329,26 @@ // STABILZE Angle Control // #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 0.54 // .48 = 4.0 NG, .54 = 4.5 NG +# define STABILIZE_ROLL_P 0.48 // .48 = 4.0 NG, .54 = 4.5 NG #endif #ifndef STABILIZE_ROLL_I # define STABILIZE_ROLL_I 0.025 // #endif #ifndef STABILIZE_ROLL_D -# define STABILIZE_ROLL_D 0.12 +# define STABILIZE_ROLL_D 0.18 #endif #ifndef STABILIZE_ROLL_IMAX # define STABILIZE_ROLL_IMAX .5 // degrees * 100 #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 0.54 +# define STABILIZE_PITCH_P 0.48 #endif #ifndef STABILIZE_PITCH_I # define STABILIZE_PITCH_I 0.025 #endif #ifndef STABILIZE_PITCH_D -# define STABILIZE_PITCH_D 0.12 +# define STABILIZE_PITCH_D 0.18 #endif #ifndef STABILIZE_PITCH_IMAX # define STABILIZE_PITCH_IMAX .5 // degrees * 100 @@ -403,16 +403,16 @@ // Navigation control gains // #ifndef NAV_LOITER_P -# define NAV_LOITER_P 2.5 // upped to be a bit more aggressive +# define NAV_LOITER_P 1.2 // upped to be a bit more aggressive #endif #ifndef NAV_LOITER_I # define NAV_LOITER_I 0.08 // upped a bit to deal with wind faster #endif #ifndef NAV_LOITER_D -# define NAV_LOITER_D 0.15 // +# define NAV_LOITER_D 0.8 // #endif #ifndef NAV_LOITER_IMAX -# define NAV_LOITER_IMAX 20 // 20° +# define NAV_LOITER_IMAX 15 // 20° #endif diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index a983c72981..6f6758dd09 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -56,6 +56,7 @@ void read_trim_switch() }else{ // switch is disengaged if(trim_flag){ + // set the throttle nominal if(g.rc_3.control_in > 150){ g.throttle_cruise.set_and_save(g.rc_3.control_in); @@ -69,11 +70,16 @@ void read_trim_switch() void auto_trim() { if(auto_level_counter > 0){ + g.rc_1.dead_zone = 60; // 60 = .6 degrees + g.rc_2.dead_zone = 60; + auto_level_counter--; trim_accel(); led_mode = AUTO_TRIM_LEDS; if(auto_level_counter == 1){ + g.rc_1.dead_zone = 0; // 60 = .6 degrees + g.rc_2.dead_zone = 0; led_mode = NORMAL_LEDS; clear_leds(); imu.save(); diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index c2e79cc412..68c62124e9 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -115,7 +115,7 @@ #define SIMPLE 2 // #define ALT_HOLD 3 // AUTO control #define AUTO 4 // AUTO control -#define GCS_AUTO 5 // AUTO control +#define GUIDED 5 // AUTO control #define LOITER 6 // Hold a single location #define RTL 7 // AUTO control #define NUM_MODES 8 diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 92c6993fd4..eb5e082084 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -62,15 +62,16 @@ void calc_loiter_nav() // Y PITCH lat_error = current_loc.lat - next_WP.lat; // 0 - 500 = -500 pitch NORTH - long_error = constrain(long_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error - lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error + // constrain input, not output to let I term ramp up and do it's job again wind + long_error = constrain(long_error, -loiter_error_max, loiter_error_max); // +- 20m max error + lat_error = constrain(lat_error, -loiter_error_max, loiter_error_max); // +- 20m max error nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav, 1.0); // X 700 * 2.5 = 1750, nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav, 1.0); // Y invert lat (for pitch) - long pmax = g.pitch_max.get(); - nav_lon = constrain(nav_lon, -pmax, pmax); - nav_lat = constrain(nav_lat, -pmax, pmax); + //long pmax = g.pitch_max.get(); + //nav_lon = constrain(nav_lon, -pmax, pmax); + //nav_lat = constrain(nav_lat, -pmax, pmax); } void calc_loiter_output() @@ -110,9 +111,9 @@ void calc_loiter_output() void calc_simple_nav() { // no dampening here in SIMPLE mode - nav_lat = constrain((wp_distance * 100), -1800, 1800); // +- 20m max error + nav_lat = constrain((wp_distance * 100), -4500, 4500); // +- 20m max error // Scale response by kP - nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° + //nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° } void calc_nav_output() diff --git a/ArduCopterMega/radio.pde b/ArduCopterMega/radio.pde index 45c56ae14e..f84fce2bfe 100644 --- a/ArduCopterMega/radio.pde +++ b/ArduCopterMega/radio.pde @@ -19,9 +19,10 @@ void init_rc_in() g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW); // set rc dead zones - g.rc_1.dead_zone = 60; // 60 = .6 degrees - g.rc_2.dead_zone = 60; + g.rc_1.dead_zone = 0; // 60 = .6 degrees + g.rc_2.dead_zone = 0; g.rc_3.dead_zone = 60; + #if YAW_OPTION == 1 g.rc_4.dead_zone = 500;// 1 = offset Yaw approach #else diff --git a/ArduCopterMega/read_me.text b/ArduCopterMega/read_me.text index f90dba98a9..564d70abf3 100644 --- a/ArduCopterMega/read_me.text +++ b/ArduCopterMega/read_me.text @@ -57,14 +57,14 @@ Set these up in 'setup'/'modes'. Use your three position switch (channel 5) to s All of the modes allow the user to control the copter with the normal controls. You can get yourself out of a jam sometimes by simply nudging the copter while in AUTO or LOITER modes. Options include: ACRO - rate control only. not for beginners -STABILIZE - copter will hold -45 to 45° angle, throttle is manual. -SIMPLE - Remembers the orientation of the copter when first entering SIMPLE mode, allowing the user to fly more intuitivey. Manual Throttle. +STABILIZE - copter will hold any angle from -45 to 45° based on roll and pitch stick input. Manual throttle. +SIMPLE - Remembers the yaw orientation of the copter when first entering SIMPLE mode, allowing the user to fly more intuitivey. Manual throttle. ALT_HOLD - altitude is controlled by the throttle lever. Middle is hold, high = rise, low = fall. LOITER - When selected, it will hold the current altitude, position and yaw. Yaw is user controllable. roll and pitch can be overridden temporarily with the radio. altitude is controlled by the throttle lever. Middle is hold, high = rise, low = fall. RTL - Will try and fly back to home at the current altitude. AUTO - Will fly the mission loaded by the Waypoint writer -GCS_AUTO - A future mode where the copter can be flown interactively from the GCS +GUIDED - A future mode where the copter can be flown interactively from the GCS Special note: diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 1e709338b2..6e91221792 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -40,7 +40,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "AC 2.0.31 Beta", main_menu_commands); +MENU(main_menu, "AC 2.0.32 Beta", main_menu_commands); void init_ardupilot() { @@ -332,6 +332,10 @@ void startup_ground(void) report_gps(); g_gps->idleTimeout = 20000; + // used to limit the input of error for loiter + loiter_error_max = (float)g.pitch_max.get() / (float)g.pid_nav_lat.kP(); + //Serial.printf_P(PSTR("\nloiter: %d\n"), loiter_error_max); + Log_Write_Startup(); SendDebug("\nReady to FLY "); @@ -391,6 +395,10 @@ void set_mode(byte mode) do_loiter_at_location(); break; + case GUIDED: + set_next_WP(&guided_WP); + break; + case RTL: //init_throttle_cruise(); do_RTL(); @@ -485,11 +493,17 @@ init_throttle_cruise() boolean check_startup_for_CLI() { - if(abs(g.rc_4.control_in) > 3000){ - // startup to fly - return true; - }else{ + //return true; + if((g.rc_4.radio_max) < 1600){ // CLI mode + return true; + + }else if(abs(g.rc_4.control_in) > 3000){ + // CLI mode + return true; + + }else{ + // startup to fly return false; } }