Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
jasonshort 2011-07-02 22:44:59 +00:00
parent 2605c4b4b2
commit a7a4f999e3
14 changed files with 294 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopterMega/APM_Config.h
View File

@ -5,6 +5,7 @@
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD //#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 #define FRAME_CONFIG QUAD_FRAME
/* /*

38
ArduCopterMega/ArduCopterMega.pde
View File

@ -227,10 +227,6 @@ GPS *g_gps;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Global variables // Global variables
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
byte control_mode = STABILIZE;
byte oldSwitchPosition; // for remembering the control mode switch
const char *comma = ","; const char *comma = ",";
const char* flight_mode_strings[] = { const char* flight_mode_strings[] = {
@ -239,7 +235,7 @@ const char* flight_mode_strings[] = {
"SIMPLE", "SIMPLE",
"ALT_HOLD", "ALT_HOLD",
"AUTO", "AUTO",
"GCS_AUTO", "GUIDED",
"LOITER", "LOITER",
"RTL"}; "RTL"};
@ -257,8 +253,9 @@ const char* flight_mode_strings[] = {
// Radio // Radio
// ----- // -----
int motor_out[8]; byte control_mode = STABILIZE;
Vector3f omega; byte oldSwitchPosition; // for remembering the control mode switch
int motor_out[8];
// Heli // 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 boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold
byte yaw_debug; byte yaw_debug;
bool did_clear_yaw_control; bool did_clear_yaw_control;
Vector3f omega;
// LED output // LED output
// ---------- // ----------
@ -335,7 +333,7 @@ float crosstrack_error; // meters we are off trackline
long distance_error; // distance to the WP long distance_error; // distance to the WP
long yaw_error; // how off are we pointed long yaw_error; // how off are we pointed
long long_error, lat_error; // temp for debugging long long_error, lat_error; // temp for debugging
int loiter_error_max;
// Battery Sensors // Battery Sensors
// --------------- // ---------------
float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter 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 target_WP; // where do we want to you towards?
struct Location simple_WP; // struct Location simple_WP; //
struct Location next_command; // command preloaded struct Location next_command; // command preloaded
struct Location guided_WP; // guided mode waypoint
long target_altitude; // used for long target_altitude; // used for
boolean home_is_set; // Flag for if we have g_gps lock and have set the home location 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.lat = 0;
simple_WP.lng = 0; simple_WP.lng = 0;
next_WP.lng = (float)g.rc_1.control_in *.4; // X: 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 *.4); // Y: 4500 / 2 = 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 // calc a new bearing
nav_bearing = get_bearing(&simple_WP, &next_WP) + initial_simple_bearing; nav_bearing = get_bearing(&simple_WP, &next_WP) + initial_simple_bearing;
nav_bearing = wrap_360(nav_bearing); nav_bearing = wrap_360(nav_bearing);
wp_distance = get_distance(&simple_WP, &next_WP); wp_distance = get_distance(&simple_WP, &next_WP);
calc_bearing_error(); calc_bearing_error();
/* /*
Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ", Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ",
next_WP.lat, next_WP.lat,
@ -1179,6 +1181,7 @@ void update_current_flight_mode(void)
output_stabilize_pitch(); output_stabilize_pitch();
break; break;
case GUIDED:
case RTL: case RTL:
// Output Pitch, Roll, Yaw and Throttle // Output Pitch, Roll, Yaw and Throttle
// ------------------------------------ // ------------------------------------
@ -1252,6 +1255,10 @@ void update_navigation()
case RTL: case RTL:
// calculates desired Yaw // calculates desired Yaw
update_nav_yaw(); update_nav_yaw();
case GUIDED:
update_nav_yaw();
// switch passthrough to LOITER
case LOITER: case LOITER:
// are we Traversing or Loitering? // are we Traversing or Loitering?
wp_control = (wp_distance < 50) ? LOITER_MODE : WP_MODE; wp_control = (wp_distance < 50) ? LOITER_MODE : WP_MODE;
@ -1325,9 +1332,11 @@ void update_alt()
int temp_sonar = sonar.read(); int temp_sonar = sonar.read();
// spike filter // spike filter
if((temp_sonar - sonar_alt) < 50){ //if((temp_sonar - sonar_alt) < 50){
sonar_alt = temp_sonar; // sonar_alt = temp_sonar;
} //}
sonar_alt = temp_sonar;
/* /*
doesn't really seem to be a need for this using EZ0: doesn't really seem to be a need for this using EZ0:
@ -1337,7 +1346,7 @@ void update_alt()
*/ */
if(baro_alt < 800){ if(baro_alt < 800){
scale = (sonar_alt - 300) / 300; scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1); 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;
@ -1430,6 +1439,7 @@ void tuning(){
void update_nav_wp() void update_nav_wp()
{ {
// XXX Guided mode!!!
if(wp_control == LOITER_MODE){ if(wp_control == LOITER_MODE){
// calc a pitch to the target // calc a pitch to the target
calc_loiter_nav(); calc_loiter_nav();

267
ArduCopterMega/GCS_Mavlink.pde
View File

@ -39,16 +39,6 @@ GCS_MAVLINK::init(BetterStream * port)
chan = MAVLINK_COMM_1; chan = MAVLINK_COMM_1;
} }
_queued_parameter = NULL; _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 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 void
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) 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) 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) { switch (msg->msgid) {
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: 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_request_data_stream_t packet;
mavlink_msg_request_data_stream_decode(msg, &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; break;
int freq = 0; // packet frequency int freq = 0; // packet frequency
@ -219,7 +199,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
streamRateExtra2 = freq; 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.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!! streamRateExtra3 = freq; // Don't save!!
break; break;
case MAV_DATA_STREAM_RAW_SENSORS: case MAV_DATA_STREAM_RAW_SENSORS:
@ -238,9 +217,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_DATA_STREAM_RAW_CONTROLLER: case MAV_DATA_STREAM_RAW_CONTROLLER:
streamRateRawController = freq; streamRateRawController = freq;
break; break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
// streamRateRawSensorFusion.set_and_save(freq); //case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
// break; // streamRateRawSensorFusion.set_and_save(freq);
// break;
case MAV_DATA_STREAM_POSITION: case MAV_DATA_STREAM_POSITION:
streamRatePosition = freq; streamRatePosition = freq;
break; break;
@ -251,7 +232,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_DATA_STREAM_EXTRA2: case MAV_DATA_STREAM_EXTRA2:
streamRateExtra2 = freq; streamRateExtra2 = freq;
//streamRateExtra2.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_EXTRA3: 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_MAG:
case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE: 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(); //startup_IMU_ground();
//result=1; //result=1;
break; // break;
/* For future implemtation /* For future implemtation
case MAV_ACTION_REC_START: break; case MAV_ACTION_REC_START: break;
@ -384,6 +364,45 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; 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: case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{ {
//send_text_P(SEVERITY_LOW,PSTR("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 // decode
mavlink_waypoint_request_list_t packet; mavlink_waypoint_request_list_t packet;
mavlink_msg_waypoint_request_list_decode(msg, &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; break;
// Start sending waypoints // Start sending waypoints
@ -415,14 +434,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
//send_text_P(SEVERITY_LOW,PSTR("waypoint request")); //send_text_P(SEVERITY_LOW,PSTR("waypoint request"));
// Check if sending waypiont // Check if sending waypiont
if (!waypoint_sending) //if (!waypoint_sending) break;
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 // decode
mavlink_waypoint_request_t packet; mavlink_waypoint_request_t packet;
mavlink_msg_waypoint_request_decode(msg, &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; break;
// send waypoint // send waypoint
@ -442,7 +461,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// time that the mav should loiter in milliseconds // time that the mav should loiter in milliseconds
uint8_t current = 0; // 1 (true), 0 (false) uint8_t current = 0; // 1 (true), 0 (false)
if (packet.seq == g.waypoint_index) if (packet.seq == (uint16_t)g.waypoint_index)
current = 1; current = 1;
uint8_t autocontinue = 1; // 1 (true), 0 (false) uint8_t autocontinue = 1; // 1 (true), 0 (false)
@ -471,7 +490,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_CMD_NAV_LOITER_TIME: 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; break;
case MAV_CMD_CONDITION_DELAY: case MAV_CMD_CONDITION_DELAY:
@ -534,9 +553,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_msg_waypoint_ack_decode(msg, &packet); mavlink_msg_waypoint_ack_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;
// check for error
uint8_t type = packet.type; // ok (0), error(1)
// turn off waypoint send // turn off waypoint send
waypoint_sending = false; waypoint_sending = false;
break; break;
@ -621,20 +637,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// XXX receive a WP from GCS and store in EEPROM // XXX receive a WP from GCS and store in EEPROM
case MAVLINK_MSG_ID_WAYPOINT: case MAVLINK_MSG_ID_WAYPOINT:
{ {
// Check if receiving waypiont
if (!waypoint_receiving) break;
// decode // decode
mavlink_waypoint_t packet; mavlink_waypoint_t packet;
mavlink_msg_waypoint_decode(msg, &packet); mavlink_msg_waypoint_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;
// 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 // defaults
tell_command.id = packet.command; tell_command.id = packet.command;
@ -728,13 +735,39 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; 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 // update waypoint receiving state machine
waypoint_timelast_receive = millis(); waypoint_timelast_receive = millis();
waypoint_request_i++; 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) uint8_t type = 0; // ok (0), error(1)
mavlink_msg_waypoint_ack_send( mavlink_msg_waypoint_ack_send(
@ -747,6 +780,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
waypoint_receiving = false; waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can // XXX ignores waypoint radius for individual waypoints, can
// only set WP_RADIUS parameter // only set WP_RADIUS parameter
}
} }
break; break;
} }
@ -759,8 +793,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// decode // decode
mavlink_param_set_t packet; mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &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 // set parameter
@ -777,7 +812,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// add a small amount before casting parameter values // add a small amount before casting parameter values
// from float to integer to avoid truncating to the // from float to integer to avoid truncating to the
// next lower integer value. // next lower integer value.
const float rounding_addition = 0.01; float rounding_addition = 0.01;
// fetch the variable type ID // fetch the variable type ID
var_type = vp->meta_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); ((AP_Float16 *)vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_int32) { } 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) { } 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) { } 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 { } else {
// we don't support mavlink set on this parameter // we don't support mavlink set on this parameter
break; break;
@ -816,16 +852,20 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break; break;
} // end case } // end case
/*
#if ALLOW_RC_OVERRIDE == ENABLED case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
{ {
// allow override of RC channel values for HIL // allow override of RC channel values for HIL
// or for complete GCS control of switch position // or for complete GCS control of switch position
// and RC PWM values. // 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]; 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[0] = packet.chan1_raw;
v[1] = packet.chan2_raw; v[1] = packet.chan2_raw;
v[2] = packet.chan3_raw; v[2] = packet.chan3_raw;
@ -834,12 +874,109 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
v[5] = packet.chan6_raw; v[5] = packet.chan6_raw;
v[6] = packet.chan7_raw; v[6] = packet.chan7_raw;
v[7] = packet.chan8_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; break;
} }
#endif #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 switch
} // end handle mavlink } // end handle mavlink

3
ArduCopterMega/Log.pde
View File

@ -73,7 +73,7 @@ print_log_menu(void)
Serial.println(); Serial.println();
if (last_log_num == 0) { if (last_log_num == 0) {
Serial.printf_P(PSTR("\nNo logs\n")); Serial.printf_P(PSTR("\nNo logs\nType 'dump 0'.\n\n"));
}else{ }else{
Serial.printf_P(PSTR("\n%d logs\n"), last_log_num); 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)) { if (/*(argc != 2) || */ (dump_log < 1) || (dump_log > last_log_num)) {
Serial.printf_P(PSTR("bad log # %d\n"), dump_log); Serial.printf_P(PSTR("bad log # %d\n"), dump_log);
Log_Read(1, 4095); Log_Read(1, 4095);
erase_logs(NULL, NULL);
return(-1); return(-1);
} }

21
ArduCopterMega/Mavlink_Common.h
View File

@ -56,6 +56,9 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
mode = MAV_MODE_AUTO; mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING; nav_mode = MAV_NAV_RETURNING;
break; break;
case GUIDED:
mode = MAV_MODE_GUIDED;
break;
default: default:
mode = control_mode + 100; 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; 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 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( mavlink_msg_sys_status_send(
chan, 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[2],
motor_out[3], motor_out[3],
motor_out[4], motor_out[4],
g.rc_5.radio_out, motor_out[7],
g.rc_6.radio_out, motor_out[8],
motor_out[7]); 0);
break; 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_y,
compass.mag_z); compass.mag_z);
mavlink_msg_raw_pressure_send( /* This message is not working. Why?
mavlink_msg_scaled_pressure_send(
chan, chan,
timeStamp, timeStamp,
adc.Ch(AIRSPEED_CH), (float)barometer.Press/100.,
barometer.RawPress, (float)adc.Ch(AIRSPEED_CH), // We don't calculate the differential pressure value anywhere, so use raw
0, (int)(barometer.Temp*100));
0); */
break; break;
} }
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE #endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE

12
ArduCopterMega/commands.pde
View File

@ -164,8 +164,8 @@ It precalculates all the necessary stuff.
void set_next_WP(struct Location *wp) void set_next_WP(struct Location *wp)
{ {
SendDebug("MSG <set_next_wp> wp_index: "); //SendDebug("MSG <set_next_wp> wp_index: ");
SendDebugln(g.waypoint_index, DEC); //SendDebugln(g.waypoint_index, DEC);
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
// copy the current WP into the OldWP slot // copy the current WP into the OldWP slot
@ -180,11 +180,6 @@ void set_next_WP(struct Location *wp)
// ----------------------------------------------------------------- // -----------------------------------------------------------------
target_altitude = current_loc.alt; 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 // this is used to offset the shrinking longitude as we go towards the poles
float rads = (abs(next_WP.lat)/t7) * 0.0174532925; float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads); scaleLongDown = cos(rads);
@ -192,8 +187,6 @@ void set_next_WP(struct Location *wp)
// this is handy for the groundstation // this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP); wp_totalDistance = get_distance(&current_loc, &next_WP);
// this makes the data not log for a GPS read
wp_distance = wp_totalDistance; wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP); target_bearing = get_bearing(&current_loc, &next_WP);
nav_bearing = target_bearing; nav_bearing = target_bearing;
@ -209,6 +202,7 @@ void set_next_WP(struct Location *wp)
gcs.print_current_waypoints(); gcs.print_current_waypoints();
} }
// run this at setup on the ground // run this at setup on the ground
// ------------------------------- // -------------------------------
void init_home() void init_home()

6
ArduCopterMega/commands_process.pde
View File

@ -94,7 +94,7 @@ process_next_command()
// Save CMD to Log // Save CMD to Log
if (g.log_bitmask & MASK_LOG_CMD) 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 // Act on the new command
process_must(); process_must();
@ -117,7 +117,7 @@ process_next_command()
// Save CMD to Log // Save CMD to Log
if (g.log_bitmask & MASK_LOG_CMD) 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(); process_may();
return true; return true;
@ -130,7 +130,7 @@ process_next_command()
//SendDebugln(next_command.id,DEC); //SendDebugln(next_command.id,DEC);
if (g.log_bitmask & MASK_LOG_CMD) 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(); process_now();
return true; return true;
} }

14
ArduCopterMega/config.h
View File

@ -329,26 +329,26 @@
// STABILZE Angle Control // STABILZE Angle Control
// //
#ifndef STABILIZE_ROLL_P #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 #endif
#ifndef STABILIZE_ROLL_I #ifndef STABILIZE_ROLL_I
# define STABILIZE_ROLL_I 0.025 // # define STABILIZE_ROLL_I 0.025 //
#endif #endif
#ifndef STABILIZE_ROLL_D #ifndef STABILIZE_ROLL_D
# define STABILIZE_ROLL_D 0.12 # define STABILIZE_ROLL_D 0.18
#endif #endif
#ifndef STABILIZE_ROLL_IMAX #ifndef STABILIZE_ROLL_IMAX
# define STABILIZE_ROLL_IMAX .5 // degrees * 100 # define STABILIZE_ROLL_IMAX .5 // degrees * 100
#endif #endif
#ifndef STABILIZE_PITCH_P #ifndef STABILIZE_PITCH_P
# define STABILIZE_PITCH_P 0.54 # define STABILIZE_PITCH_P 0.48
#endif #endif
#ifndef STABILIZE_PITCH_I #ifndef STABILIZE_PITCH_I
# define STABILIZE_PITCH_I 0.025 # define STABILIZE_PITCH_I 0.025
#endif #endif
#ifndef STABILIZE_PITCH_D #ifndef STABILIZE_PITCH_D
# define STABILIZE_PITCH_D 0.12 # define STABILIZE_PITCH_D 0.18
#endif #endif
#ifndef STABILIZE_PITCH_IMAX #ifndef STABILIZE_PITCH_IMAX
# define STABILIZE_PITCH_IMAX .5 // degrees * 100 # define STABILIZE_PITCH_IMAX .5 // degrees * 100
@ -403,16 +403,16 @@
// Navigation control gains // Navigation control gains
// //
#ifndef NAV_LOITER_P #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 #endif
#ifndef NAV_LOITER_I #ifndef NAV_LOITER_I
# define NAV_LOITER_I 0.08 // upped a bit to deal with wind faster # define NAV_LOITER_I 0.08 // upped a bit to deal with wind faster
#endif #endif
#ifndef NAV_LOITER_D #ifndef NAV_LOITER_D
# define NAV_LOITER_D 0.15 // # define NAV_LOITER_D 0.8 //
#endif #endif
#ifndef NAV_LOITER_IMAX #ifndef NAV_LOITER_IMAX
# define NAV_LOITER_IMAX 20 // 20° # define NAV_LOITER_IMAX 15 // 20°
#endif #endif

6
ArduCopterMega/control_modes.pde
View File

@ -56,6 +56,7 @@ void read_trim_switch()
}else{ // switch is disengaged }else{ // switch is disengaged
if(trim_flag){ if(trim_flag){
// set the throttle nominal // set the throttle nominal
if(g.rc_3.control_in > 150){ if(g.rc_3.control_in > 150){
g.throttle_cruise.set_and_save(g.rc_3.control_in); g.throttle_cruise.set_and_save(g.rc_3.control_in);
@ -69,11 +70,16 @@ void read_trim_switch()
void auto_trim() void auto_trim()
{ {
if(auto_level_counter > 0){ if(auto_level_counter > 0){
g.rc_1.dead_zone = 60; // 60 = .6 degrees
g.rc_2.dead_zone = 60;
auto_level_counter--; auto_level_counter--;
trim_accel(); trim_accel();
led_mode = AUTO_TRIM_LEDS; led_mode = AUTO_TRIM_LEDS;
if(auto_level_counter == 1){ if(auto_level_counter == 1){
g.rc_1.dead_zone = 0; // 60 = .6 degrees
g.rc_2.dead_zone = 0;
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
clear_leds(); clear_leds();
imu.save(); imu.save();

2
ArduCopterMega/defines.h
View File

@ -115,7 +115,7 @@
#define SIMPLE 2 // #define SIMPLE 2 //
#define ALT_HOLD 3 // AUTO control #define ALT_HOLD 3 // AUTO control
#define AUTO 4 // 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 LOITER 6 // Hold a single location
#define RTL 7 // AUTO control #define RTL 7 // AUTO control
#define NUM_MODES 8 #define NUM_MODES 8

15
ArduCopterMega/navigation.pde
View File

@ -62,15 +62,16 @@ void calc_loiter_nav()
// Y PITCH // Y PITCH
lat_error = current_loc.lat - next_WP.lat; // 0 - 500 = -500 pitch NORTH 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 // constrain input, not output to let I term ramp up and do it's job again wind
lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error 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_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) nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav, 1.0); // Y invert lat (for pitch)
long pmax = g.pitch_max.get(); //long pmax = g.pitch_max.get();
nav_lon = constrain(nav_lon, -pmax, pmax); //nav_lon = constrain(nav_lon, -pmax, pmax);
nav_lat = constrain(nav_lat, -pmax, pmax); //nav_lat = constrain(nav_lat, -pmax, pmax);
} }
void calc_loiter_output() void calc_loiter_output()
@ -110,9 +111,9 @@ void calc_loiter_output()
void calc_simple_nav() void calc_simple_nav()
{ {
// no dampening here in SIMPLE mode // 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 // 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() void calc_nav_output()

5
ArduCopterMega/radio.pde
View File

@ -19,9 +19,10 @@ void init_rc_in()
g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW); g.rc_4.set_type(RC_CHANNEL_ANGLE_RAW);
// set rc dead zones // set rc dead zones
g.rc_1.dead_zone = 60; // 60 = .6 degrees g.rc_1.dead_zone = 0; // 60 = .6 degrees
g.rc_2.dead_zone = 60; g.rc_2.dead_zone = 0;
g.rc_3.dead_zone = 60; g.rc_3.dead_zone = 60;
#if YAW_OPTION == 1 #if YAW_OPTION == 1
g.rc_4.dead_zone = 500;// 1 = offset Yaw approach g.rc_4.dead_zone = 500;// 1 = offset Yaw approach
#else #else

6
ArduCopterMega/read_me.text
View File

@ -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. 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: Options include:
ACRO - rate control only. not for beginners ACRO - rate control only. not for beginners
STABILIZE - copter will hold -45 to 45° angle, throttle is manual. STABILIZE - copter will hold any angle from -45 to 45° based on roll and pitch stick input. Manual throttle.
SIMPLE - Remembers the orientation of the copter when first entering SIMPLE mode, allowing the user to fly more intuitivey. 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. 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. 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. 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. RTL - Will try and fly back to home at the current altitude.
AUTO - Will fly the mission loaded by the Waypoint writer 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: Special note:

24
ArduCopterMega/system.pde
View File

@ -40,7 +40,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
}; };
// Create the top-level menu object. // 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() void init_ardupilot()
{ {
@ -332,6 +332,10 @@ void startup_ground(void)
report_gps(); report_gps();
g_gps->idleTimeout = 20000; 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(); Log_Write_Startup();
SendDebug("\nReady to FLY "); SendDebug("\nReady to FLY ");
@ -391,6 +395,10 @@ void set_mode(byte mode)
do_loiter_at_location(); do_loiter_at_location();
break; break;
case GUIDED:
set_next_WP(&guided_WP);
break;
case RTL: case RTL:
//init_throttle_cruise(); //init_throttle_cruise();
do_RTL(); do_RTL();
@ -485,11 +493,17 @@ init_throttle_cruise()
boolean boolean
check_startup_for_CLI() check_startup_for_CLI()
{ {
if(abs(g.rc_4.control_in) > 3000){ //return true;
// startup to fly if((g.rc_4.radio_max) < 1600){
return true;
}else{
// CLI mode // CLI mode
return true;
}else if(abs(g.rc_4.control_in) > 3000){
// CLI mode
return true;
}else{
// startup to fly
return false; return false;
} }
} }