mirror of https://github.com/ArduPilot/ardupilot
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
parent
2605c4b4b2
commit
a7a4f999e3
|
|
@ -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
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -164,8 +164,8 @@ It precalculates all the necessary stuff.
|
|||
|
||||
void set_next_WP(struct Location *wp)
|
||||
{
|
||||
SendDebug("MSG <set_next_wp> wp_index: ");
|
||||
SendDebugln(g.waypoint_index, DEC);
|
||||
//SendDebug("MSG <set_next_wp> 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()
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue