Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Big changes in the way commands are parsed and mission tasks are done. Not tested at all.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1736 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-03-03 02:32:50 +00:00
parent df72a4a5aa
commit b59474fc1a
14 changed files with 586 additions and 423 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
ArduCopterMega/ArduCopterMega.pde
View File

@ -139,6 +139,7 @@ GPS *g_gps;
#error Unrecognised HIL_MODE setting.
#endif // HIL MODE
// HIL
#if HIL_MODE != HIL_MODE_DISABLED
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
GCS_MAVLINK hil;
@ -149,11 +150,14 @@ GPS *g_gps;
#if HIL_MODE != HIL_MODE_ATTITUDE
#if HIL_MODE != HIL_MODE_SENSORS
AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration); // normal imu
// Normal
AP_IMU_Oilpan imu(&adc, Parameters::k_param_IMU_calibration);
#else
AP_IMU_Shim imu; // hil imu
// hil imu
AP_IMU_Shim imu;
#endif
AP_DCM dcm(&imu, g_gps); // normal dcm
// normal dcm
AP_DCM dcm(&imu, g_gps);
#endif
////////////////////////////////////////////////////////////////////////////////
@ -184,8 +188,8 @@ const char* flight_mode_strings[] = {
"ALT_HOLD",
"FBW",
"AUTO",
"LOITER",
"POSITION_HOLD",
"GCS_AUTO",
"POS_HOLD",
"RTL",
"TAKEOFF",
"LAND"};
@ -296,7 +300,7 @@ byte altitude_sensor = BARO; // used to know which sensor is active, BARO or
// --------------------
boolean takeoff_complete; // Flag for using take-off controls
boolean land_complete;
int takeoff_altitude;
//int takeoff_altitude;
int landing_distance; // meters;
long old_alt; // used for managing altitude rates
int velocity_land;
@ -356,7 +360,7 @@ struct Location home; // home location
struct Location prev_WP; // last waypoint
struct Location current_loc; // current location
struct Location next_WP; // next waypoint
struct Location tell_command; // command for telemetry
//struct Location tell_command; // command for telemetry
struct Location next_command; // command preloaded
long target_altitude; // used for
long offset_altitude; // used for
@ -542,7 +546,9 @@ void medium_loop()
// perform next command
// --------------------
update_commands();
if(control_mode == AUTO || control_mode == GCS_AUTO){
update_commands();
}
break;
// This case deals with sending high rate telemetry
@ -683,7 +689,6 @@ void super_slow_loop()
Log_Write_Current();
}
void update_GPS(void)
{
g_gps->update();
@ -800,7 +805,7 @@ void update_current_flight_mode(void)
}
break;
case LOITER:
//case LOITER:
case STABILIZE:
// clear any AP naviagtion values
nav_pitch = 0;
@ -905,7 +910,7 @@ void update_current_flight_mode(void)
// apply throttle control
output_auto_throttle();
// mix in user control
// mix in user control with Nav control
control_nav_mixer();
// perform stabilzation
@ -925,7 +930,7 @@ void update_current_flight_mode(void)
// apply throttle control
output_auto_throttle();
// mix in user control
// mix in user control with Nav control
control_nav_mixer();
// perform stabilzation
@ -948,9 +953,8 @@ void update_navigation()
// ------------------------------------------------------------------------
// distance and bearing calcs only
if(control_mode == AUTO){
verify_must();
verify_may();
if(control_mode == AUTO || control_mode == GCS_AUTO){
verify_commands();
}else{
switch(control_mode){
case RTL:

1
ArduCopterMega/Attitude.pde
View File

@ -246,7 +246,6 @@ void output_manual_yaw()
}else{
// Yaw control
if(g.rc_4.control_in == 0){
//clear_yaw_control();
output_yaw_with_hold(true); // hold yaw
}else{
output_yaw_with_hold(false); // rate control yaw

4
ArduCopterMega/GCS_Mavlink.pde
View File

@ -213,7 +213,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_ACTION_HALT:
loiter_at_location();
do_hold_position();
break;
// can't implement due to APM configuration
@ -285,7 +285,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_ACTION_LOITER:
set_mode(LOITER);
//set_mode(LOITER);
break;
default: break;

259
ArduCopterMega/Mavlink_Common.h
View File

@ -27,164 +27,242 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
switch(id) {
case MSG_HEARTBEAT:
mavlink_msg_heartbeat_send(chan,system_type,MAV_AUTOPILOT_ARDUPILOTMEGA);
mavlink_msg_heartbeat_send(
chan,
system_type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
break;
case MSG_EXTENDED_STATUS:
{
uint8_t mode = MAV_MODE_UNINIT;
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
switch(control_mode) {
case MANUAL:
mode = MAV_MODE_MANUAL;
mode = MAV_MODE_MANUAL;
break;
case CIRCLE:
mode = MAV_MODE_TEST3;
mode = MAV_MODE_TEST3;
break;
case STABILIZE:
mode = MAV_MODE_GUIDED;
mode = MAV_MODE_GUIDED;
break;
case FLY_BY_WIRE_A:
mode = MAV_MODE_TEST1;
mode = MAV_MODE_TEST1;
break;
case FLY_BY_WIRE_B:
mode = MAV_MODE_TEST2;
mode = MAV_MODE_TEST2;
break;
case AUTO:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_WAYPOINT;
break;
case RTL:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_RETURNING;
break;
case LOITER:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_HOLD;
break;
case TAKEOFF:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LIFTOFF;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LIFTOFF;
break;
case LAND:
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LANDING;
mode = MAV_MODE_AUTO;
nav_mode = MAV_NAV_LANDING;
break;
}
uint8_t status = MAV_STATE_ACTIVE;
uint8_t status = MAV_STATE_ACTIVE;
uint8_t motor_block = false;
mavlink_msg_sys_status_send(chan,mode,nav_mode,status,load*1000,
battery_voltage1*1000,motor_block,packet_drops);
mavlink_msg_sys_status_send(
chan,
mode,
nav_mode,
status,
load * 1000,
battery_voltage1 * 1000,
motor_block,
packet_drops);
break;
}
case MSG_ATTITUDE:
{
Vector3f omega = dcm.get_gyro();
mavlink_msg_attitude_send(chan,timeStamp,dcm.roll,dcm.pitch,dcm.yaw,
omega.x,omega.y,omega.z);
break;
}
mavlink_msg_attitude_send(
chan,
timeStamp,
dcm.roll,
dcm.pitch,
dcm.yaw,
omega.x,
omega.y,
omega.z);
break;
}
case MSG_LOCATION:
{
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(chan,current_loc.lat,
current_loc.lng,current_loc.alt*10,g_gps->ground_speed/1.0e2*rot.a.x,
g_gps->ground_speed/1.0e2*rot.b.x,g_gps->ground_speed/1.0e2*rot.c.x);
mavlink_msg_global_position_int_send(
chan,
current_loc.lat,
current_loc.lng,
current_loc.alt * 10,
g_gps->ground_speed / 1.0e2 * rot.a.x,
g_gps->ground_speed / 1.0e2 * rot.b.x,
g_gps->ground_speed / 1.0e2 * rot.c.x);
break;
}
case MSG_LOCAL_LOCATION:
{
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_local_position_send(chan,timeStamp,ToRad((current_loc.lat-home.lat)/1.0e7)*radius_of_earth,
ToRad((current_loc.lng-home.lng)/1.0e7)*radius_of_earth*cos(ToRad(home.lat/1.0e7)),
(current_loc.alt-home.alt)/1.0e2, g_gps->ground_speed/1.0e2*rot.a.x,
g_gps->ground_speed/1.0e2*rot.b.x,g_gps->ground_speed/1.0e2*rot.c.x);
mavlink_msg_local_position_send(
chan,
timeStamp,
ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth,
ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)),
(current_loc.alt - home.alt) / 1.0e2,
g_gps->ground_speed / 1.0e2 * rot.a.x,
g_gps->ground_speed / 1.0e2 * rot.b.x,
g_gps->ground_speed / 1.0e2 * rot.c.x);
break;
}
case MSG_GPS_RAW:
{
mavlink_msg_gps_raw_send(chan,timeStamp,g_gps->status(),
g_gps->latitude/1.0e7,g_gps->longitude/1.0e7,g_gps->altitude/100.0,
g_gps->hdop,0.0,g_gps->ground_speed/100.0,g_gps->ground_course/100.0);
break;
mavlink_msg_gps_raw_send(
chan,
timeStamp,
g_gps->status(),
g_gps->latitude / 1.0e7,
g_gps->longitude / 1.0e7,
g_gps->altitude / 100.0,
g_gps->hdop,
0.0,
g_gps->ground_speed / 100.0,
g_gps->ground_course / 100.0);
break;
}
case MSG_SERVO_OUT:
{
uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
mavlink_msg_rc_channels_scaled_send(chan,
10000*rc[0]->norm_output(),
10000*rc[1]->norm_output(),
10000*rc[2]->norm_output(),
10000*rc[3]->norm_output(),
0,0,0,0,rssi);
break;
}
case MSG_RADIO_IN:
{
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(chan,
rc[0]->radio_in,
rc[1]->radio_in,
rc[2]->radio_in,
rc[3]->radio_in,
0/*rc[4]->radio_in*/, // XXX currently only 4 RC channels defined
0/*rc[5]->radio_in*/,
0/*rc[6]->radio_in*/,
0/*rc[7]->radio_in*/,
rssi);
break;
}
case MSG_RADIO_OUT:
{
mavlink_msg_servo_output_raw_send(chan,
rc[0]->radio_out,
rc[1]->radio_out,
rc[2]->radio_out,
rc[3]->radio_out,
0/*rc[4]->radio_out*/, // XXX currently only 4 RC channels defined
0/*rc[5]->radio_out*/,
0/*rc[6]->radio_out*/,
0/*rc[7]->radio_out*/);
break;
}
case MSG_VFR_HUD:
{
mavlink_msg_vfr_hud_send(chan, (float)airspeed/100.0, (float)g_gps->ground_speed/100.0, dcm.yaw_sensor, current_loc.alt/100.0,
climb_rate, (int)rc[CH_THROTTLE]->servo_out);
mavlink_msg_rc_channels_scaled_send(
chan,
10000 * g.channel_roll.norm_output(),
10000 * g.channel_pitch.norm_output(),
10000 * g.channel_throttle.norm_output(),
10000 * g.channel_rudder.norm_output(),
0,
0,
0,
0,
rssi);
break;
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU:
case MSG_RADIO_IN:
{
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
g.channel_roll.radio_in,
g.channel_pitch.radio_in,
g.channel_throttle.radio_in,
g.channel_rudder.radio_in,
g.rc_5.radio_in, // XXX currently only 4 RC channels defined
g.rc_6.radio_in,
g.rc_7.radio_in,
g.rc_8.radio_in,
rssi);
break;
}
case MSG_RADIO_OUT:
{
mavlink_msg_servo_output_raw_send(
chan,
g.channel_roll.radio_out,
g.channel_pitch.radio_out,
g.channel_throttle.radio_out,
g.channel_rudder.radio_out,
g.rc_5.radio_out, // XXX currently only 4 RC channels defined
g.rc_6.radio_out,
g.rc_7.radio_out,
g.rc_8.radio_out);
break;
}
case MSG_VFR_HUD:
{
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
(float)g_gps->ground_speed / 100.0,
dcm.yaw_sensor,
current_loc.alt / 100.0,
climb_rate,
(int)g.channel_throttle.servo_out);
break;
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU:
{
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
//Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z);
mavlink_msg_raw_imu_send(chan,timeStamp,
accel.x*1000.0/gravity,accel.y*1000.0/gravity,accel.z*1000.0/gravity,
gyro.x*1000.0,gyro.y*1000.0,gyro.z*1000.0,
compass.mag_x,compass.mag_y,compass.mag_z);
mavlink_msg_raw_pressure_send(chan,timeStamp,
adc.Ch(AIRSPEED_CH),barometer.RawPress,0,0);
break;
}
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
mavlink_msg_raw_imu_send(
chan,
timeStamp,
accel.x * 1000.0 / gravity,
accel.y * 1000.0 / gravity,
accel.z * 1000.0 / gravity,
gyro.x * 1000.0,
gyro.y * 1000.0,
gyro.z * 1000.0,
compass.mag_x,
compass.mag_y,
compass.mag_z);
mavlink_msg_raw_pressure_send(
chan,
timeStamp,
adc.Ch(AIRSPEED_CH),
barometer.RawPress,
0,
0);
break;
}
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
case MSG_GPS_STATUS:
{
mavlink_msg_gps_status_send(chan,g_gps->num_sats,NULL,NULL,NULL,NULL,NULL);
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
NULL,
NULL,
NULL,
NULL,
NULL);
break;
}
case MSG_CURRENT_WAYPOINT:
{
mavlink_msg_waypoint_current_send(chan,g.waypoint_index);
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
break;
}
@ -195,7 +273,10 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
{
mavlink_msg_statustext_send(chan,severity,(const int8_t*)str);
mavlink_msg_statustext_send(
chan,
severity,
(const int8_t*) str);
}
void mavlink_acknowledge(mavlink_channel_t chan, uint8_t id, uint8_t sum1, uint8_t sum2)

85
ArduCopterMega/commands.pde
View File

@ -12,12 +12,12 @@ void init_commands()
void update_auto()
{
if (g.waypoint_index == g.waypoint_total) {
return_to_launch();
//wp_index = 0;
do_RTL();
}
}
void reload_commands()
// this is only used by an air-start
void reload_commands_airstart()
{
init_commands();
g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all?
@ -31,7 +31,6 @@ struct Location get_wp_with_index(int i)
struct Location temp;
long mem;
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.waypoint_total) {
@ -49,6 +48,11 @@ struct Location get_wp_with_index(int i)
mem += 4;
temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
}
// Add on home altitude if we are a nav command
if(temp.id < 50)
temp.alt += home.alt;
return temp;
}
@ -57,6 +61,12 @@ struct Location get_wp_with_index(int i)
void set_wp_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.waypoint_total.get());
if(i > 0 && temp.id < 50){
// remove home altitude if we are a nav command
temp.alt -= home.alt;
}
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
eeprom_write_byte((uint8_t *) mem, temp.id);
@ -101,69 +111,18 @@ long read_alt_to_hold()
return g.RTL_altitude + home.alt;
}
void
set_current_loc_here()
{
//struct Location temp;
Location l = current_loc;
l.alt = get_altitude_above_home();
set_next_WP(&l);
}
void loiter_at_location()
{
next_WP = current_loc;
}
void set_mode_loiter_home(void)
{
control_mode = LOITER;
//crash_timer = 0;
next_WP = current_loc;
// Altitude to hold over home
// Set by configuration tool
// -------------------------
next_WP.alt = read_alt_to_hold();
// output control mode to the ground station
gcs.send_message(MSG_HEARTBEAT);
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
}
//********************************************************************************
//This function sets the waypoint and modes for Return to Launch
//********************************************************************************
// add a new command at end of command set to RTL.
void return_to_launch(void)
{
//so we know where we are navigating from
next_WP = current_loc;
// home is WP 0
// ------------
g.waypoint_index.set_and_save(0);
// Loads WP from Memory
// --------------------
set_next_WP(&home);
// Altitude to hold over home
// Set by configuration tool
// -------------------------
next_WP.alt = read_alt_to_hold();
//send_message(SEVERITY_LOW,"Return To Launch");
}
struct Location get_LOITER_home_wp()
Location get_LOITER_home_wp()
{
// read home position
struct Location temp = get_wp_with_index(0);
temp.id = MAV_CMD_NAV_LOITER_UNLIM;
temp.alt = read_alt_to_hold() - home.alt; // will be incremented up by home.alt in set_next_WP
temp.alt = read_alt_to_hold();
return temp;
}
@ -186,14 +145,11 @@ void set_next_WP(struct Location *wp)
// ---------------------
next_WP = *wp;
// offset the altitude relative to home position
// ---------------------------------------------
next_WP.alt += home.alt;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude = current_loc.alt;
// XXX YUCK!
if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND))
offset_altitude = next_WP.alt - prev_WP.alt;
else
@ -210,10 +166,12 @@ void set_next_WP(struct Location *wp)
scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation
wp_totalDistance = getDistance(&current_loc, &next_WP);
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
// to check if we have missed the WP
// ----------------------------
old_target_bearing = target_bearing;
// set a new crosstrack bearing
@ -223,7 +181,6 @@ void set_next_WP(struct Location *wp)
gcs.print_current_waypoints();
}
// run this at setup on the ground
// -------------------------------
void init_home()

514
ArduCopterMega/commands_logic.pde
View File

@ -1,157 +1,59 @@
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
void
handle_no_commands()
{
switch (control_mode){
case LAND:
// don't get a new command
break;
default:
next_command = get_LOITER_home_wp();
//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
//SendDebugln(next_command.id,DEC);
break;
}
}
void
handle_process_must(byte id)
handle_process_must()
{
// reset navigation integrators
// -------------------------
reset_I();
switch(id){
case MAV_CMD_NAV_TAKEOFF: // TAKEOFF!
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
takeoff_altitude = (int)next_command.alt;
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
next_WP.alt = current_loc.alt + takeoff_altitude;
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
//set_next_WP(&next_WP);
switch(next_command.id){
case MAV_CMD_NAV_TAKEOFF:
do_takeoff();
break;
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
do_nav_wp();
break;
//case MAV_CMD_NAV_R_WAYPOINT: // Navigate to Waypoint
// next_command.lat += home.lat; // offset from home location
// next_command.lng += home.lng; // offset from home location
// we've recalculated the WP so we need to set it again
// set_next_WP(&next_command);
// break;
case MAV_CMD_NAV_LAND: // LAND to Waypoint
velocity_land = 1000;
next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng;
next_WP.alt = home.alt;
land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
break;
/*
case MAV_CMD_ALTITUDE: //
next_WP.lat = current_loc.lat + 10; // so we don't have bad calcs
next_WP.lng = current_loc.lng + 10; // so we don't have bad calcs
break;
*/
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
break;
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
break;
case MAV_CMD_NAV_LOITER_TIME:
do_land();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return_to_launch();
do_RTL();
break;
}
}
void
handle_process_may(byte id)
handle_process_may()
{
switch(id){
switch(next_command.id){
case MAV_CMD_CONDITION_DELAY: // Navigate to Waypoint
delay_start = millis();
delay_timeout = next_command.lat;
case MAV_CMD_CONDITION_DELAY:
do_delay();
break;
//case MAV_CMD_NAV_LAND_OPTIONS: // Land this puppy
// break;
case MAV_CMD_CONDITION_CHANGE_ALT:
do_change_alt();
break;
case MAV_CMD_CONDITION_YAW:
// p1: bearing
// alt: speed
// lat: direction (-1,1),
// lng: rel (1) abs (0)
// target angle in degrees
command_yaw_start = nav_yaw; // current position
command_yaw_start_time = millis();
// which direction to turn
// 1 = clockwise, -1 = counterclockwise
command_yaw_dir = next_command.lat;
// 1 = Relative or 0 = Absolute
if (next_command.lng == 1) {
// relative
command_yaw_dir = (command_yaw_end > 0) ? 1 : -1;
command_yaw_end += nav_yaw;
command_yaw_end = wrap_360(command_yaw_end);
}else{
// absolute
command_yaw_end = next_command.p1 * 100;
}
// if unspecified go 10° a second
if(command_yaw_speed == 0)
command_yaw_speed = 10;
// if unspecified go clockwise
if(command_yaw_dir == 0)
command_yaw_dir = 1;
// calculate the delta travel
if(command_yaw_dir == 1){
if(command_yaw_start > command_yaw_end){
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
}else{
command_yaw_delta = command_yaw_end - command_yaw_start;
}
}else{
if(command_yaw_start > command_yaw_end){
command_yaw_delta = command_yaw_start - command_yaw_end;
}else{
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
}
}
command_yaw_delta = wrap_360(command_yaw_delta);
// rate to turn deg per second - default is ten
command_yaw_speed = next_command.alt;
command_yaw_time = command_yaw_delta / command_yaw_speed;
//9000 turn in 10 seconds
//command_yaw_time = 9000/ 10 = 900° per second
do_yaw();
break;
default:
break;
}}
}
}
void
handle_process_now(byte id)
handle_process_now()
{
switch(id){
switch(next_command.id){
case MAV_CMD_DO_SET_HOME:
init_home();
break;
@ -161,125 +63,327 @@ handle_process_now(byte id)
break;
case MAV_CMD_DO_SET_SERVO:
//Serial.print("MAV_CMD_DO_SET_SERVO ");
//Serial.print(next_command.p1,DEC);
//Serial.print(" PWM: ");
//Serial.println(next_command.alt,DEC);
APM_RC.OutputCh(next_command.p1, next_command.alt);
do_set_servo();
break;
case MAV_CMD_DO_SET_RELAY:
if (next_command.p1 == 0) {
relay_on();
} else if (next_command.p1 == 1) {
relay_off();
}else{
relay_toggle();
}
do_set_relay();
break;
}
}
// Verify commands
// ---------------
void verify_must()
void
handle_no_commands()
{
switch (control_mode){
case LAND:
// don't get a new command
break;
//case GCS_AUTO:
// set_mode(LOITER);
default:
set_mode(RTL);
//next_command = get_LOITER_home_wp();
//SendDebug("MSG <load_next_command> Preload RTL cmd id: ");
//SendDebugln(next_command.id,DEC);
break;
}
}
bool verify_must()
{
switch(command_must_ID) {
/*case MAV_CMD_ALTITUDE:
if (abs(next_WP.alt - current_loc.alt) < 100){
command_must_index = 0;
}
break;
*/
case MAV_CMD_NAV_TAKEOFF: // Takeoff!
if (current_loc.alt > (next_WP.alt -100)){
command_must_index = 0;
takeoff_complete = true;
}
return verify_takeoff();
break;
case MAV_CMD_NAV_LAND:
// 10 - 9 = 1
velocity_land = ((old_alt - current_loc.alt) *.05) + (velocity_land * .95);
old_alt = current_loc.alt;
if(velocity_land == 0){
land_complete = true;
command_must_index = 0;
}
update_crosstrack();
return verify_land();
break;
case MAV_CMD_NAV_WAYPOINT: // reach a waypoint
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
//SendDebugln(command_must_index,DEC);
char message[50];
sprintf(message,"Reached Waypoint #%i",command_must_index);
gcs.send_text(SEVERITY_LOW,message);
// clear the command queue;
command_must_index = 0;
}
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
send_message(SEVERITY_MEDIUM,"Missed WP");
command_must_index = 0;
}
break;
case MAV_CMD_NAV_LOITER_TURNS: // LOITER N times
break;
case MAV_CMD_NAV_LOITER_TIME: // loiter N milliseconds
return verify_nav_wp();
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
if (wp_distance <= g.waypoint_radius) {
gcs.send_text(SEVERITY_LOW,"Reached home");
command_must_index = 0;
}
return verify_RTL();
break;
//case MAV_CMD_NAV_LOITER_UNLIM: // Just plain LOITER
// break;
default:
gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current Must commands");
//gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current Must commands");
return false;
break;
}
}
void verify_may()
bool verify_may()
{
float power;
switch(command_may_ID) {
case MAV_CMD_CONDITION_ANGLE:
if((millis() - command_yaw_start_time) > command_yaw_time){
command_must_index = 0;
nav_yaw = command_yaw_end;
}else{
// else we need to be at a certain place
// power is a ratio of the time : .5 = half done
power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time;
nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir);
}
return verify_yaw();
break;
case MAV_CMD_CONDITION_DELAY:
if ((millis() - delay_start) > delay_timeout){
command_may_index = 0;
delay_timeout = 0;
}
return verify_delay();
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt();
break;
default:
//gcs.send_text(SEVERITY_HIGH,"<verify_must: default> No current May commands");
return false;
break;
//case CMD_LAND_OPTIONS:
// break;
}
}
/********************************************************************************/
// Must command implementations
/********************************************************************************/
void
do_takeoff()
{
Location temp = current_loc;
temp.alt = next_command.alt;
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
set_next_WP(&temp);
}
bool
verify_takeoff()
{
if (current_loc.alt > next_WP.alt){
takeoff_complete = true;
return true;
}else{
return false;
}
}
void
do_nav_wp()
{
set_next_WP(&next_command);
}
bool
verify_nav_wp()
{
update_crosstrack();
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
//SendDebug("MSG <verify_must: MAV_CMD_NAV_WAYPOINT> REACHED_WAYPOINT #");
//SendDebugln(command_must_index,DEC);
char message[30];
sprintf(message,"Reached Waypoint #%i",command_must_index);
gcs.send_text(SEVERITY_LOW,message);
return true;
}
// add in a more complex case
// Doug to do
if(loiter_sum > 300){
send_message(SEVERITY_MEDIUM,"Missed WP");
return true;
}
return false;
}
void
do_land()
{
land_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
velocity_land = 1000;
Location temp = current_loc;
temp.alt = home.alt;
set_next_WP(&temp);
}
bool
verify_land()
{
update_crosstrack();
velocity_land = ((old_alt - current_loc.alt) *.05) + (velocity_land * .95);
old_alt = current_loc.alt;
if(velocity_land == 0){
land_complete = true;
return true;
}
return false;
}
// add a new command at end of command set to RTL.
void
do_RTL()
{
Location temp = home;
temp.alt = read_alt_to_hold();
//so we know where we are navigating from
next_WP = current_loc;
// Loads WP from Memory
// --------------------
set_next_WP(&temp);
}
bool
verify_RTL()
{
if (wp_distance <= g.waypoint_radius) {
gcs.send_text(SEVERITY_LOW,"Reached home");
return true;
}else{
return false;
}
}
/********************************************************************************/
// May command implementations
/********************************************************************************/
void
do_yaw()
{
// p1: bearing
// alt: speed
// lat: direction (-1,1),
// lng: rel (1) abs (0)
// target angle in degrees
command_yaw_start = nav_yaw; // current position
command_yaw_start_time = millis();
// which direction to turn
// 1 = clockwise, -1 = counterclockwise
command_yaw_dir = next_command.lat;
// 1 = Relative or 0 = Absolute
if (next_command.lng == 1) {
// relative
command_yaw_dir = (command_yaw_end > 0) ? 1 : -1;
command_yaw_end += nav_yaw;
command_yaw_end = wrap_360(command_yaw_end);
}else{
// absolute
command_yaw_end = next_command.p1 * 100;
}
// if unspecified go 10° a second
if(command_yaw_speed == 0)
command_yaw_speed = 10;
// if unspecified go clockwise
if(command_yaw_dir == 0)
command_yaw_dir = 1;
// calculate the delta travel
if(command_yaw_dir == 1){
if(command_yaw_start > command_yaw_end){
command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end);
}else{
command_yaw_delta = command_yaw_end - command_yaw_start;
}
}else{
if(command_yaw_start > command_yaw_end){
command_yaw_delta = command_yaw_start - command_yaw_end;
}else{
command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end);
}
}
command_yaw_delta = wrap_360(command_yaw_delta);
// rate to turn deg per second - default is ten
command_yaw_speed = next_command.alt;
command_yaw_time = command_yaw_delta / command_yaw_speed;
//9000 turn in 10 seconds
//command_yaw_time = 9000/ 10 = 900° per second
}
bool
verify_yaw()
{
if((millis() - command_yaw_start_time) > command_yaw_time){
nav_yaw = command_yaw_end;
return true;
}else{
// else we need to be at a certain place
// power is a ratio of the time : .5 = half done
float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time;
nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir);
return false;
}
}
void
do_delay()
{
delay_start = millis();
delay_timeout = next_command.lat;
}
bool
verify_delay()
{
if ((millis() - delay_start) > delay_timeout){
delay_timeout = 0;
return true;
}else{
return false;
}
}
void
do_change_alt()
{
Location temp = next_WP;
temp.alt = next_command.alt + home.alt;
set_next_WP(&temp);
}
bool
verify_change_alt()
{
if(abs(current_loc.alt - next_WP.alt) < 100){
return true;
}else{
return false;
}
}
/********************************************************************************/
// Now command implementations
/********************************************************************************/
void do_hold_position()
{
set_next_WP(&current_loc);
}
void do_set_servo()
{
APM_RC.OutputCh(next_command.p1, next_command.alt);
}
void do_set_relay()
{
if (next_command.p1 == 0) {
relay_on();
} else if (next_command.p1 == 1) {
relay_off();
}else{
relay_toggle();
}
}

69
ArduCopterMega/commands_process.pde
View File

@ -5,43 +5,55 @@ void update_commands(void)
// This function loads commands into three buffers
// when a new command is loaded, it is processed with process_XXX()
// ----------------------------------------------------------------
if((home_is_set == false) || (control_mode != AUTO)){
if(home_is_set == false){
return; // don't do commands
}
if(control_mode == AUTO){
load_next_command();
load_next_command_from_EEPROM();
process_next_command();
}else if(control_mode == GCS_AUTO){
/*if( there is a command recieved )
process_next_command();
*/
}
//verify_must();
//verify_may();
}
void load_next_command()
void verify_commands(void)
{
// fetch next command if it's empty
// --------------------------------
if(next_command.id == CMD_BLANK){
if(verify_must()){
command_must_index = NO_COMMAND;
}
if(verify_may()){
command_may_index = NO_COMMAND;
}
}
void load_next_command_from_EEPROM()
{
// fetch next command if the next command queue is empty
// -----------------------------------------------------
if(next_command.id == NO_COMMAND){
next_command = get_wp_with_index(g.waypoint_index + 1);
if(next_command.id != CMD_BLANK){
//if(next_command.id != NO_COMMAND){
//SendDebug("MSG <load_next_command> fetch found new cmd from list at index ");
//SendDebug((g.waypoint_index + 1),DEC);
//SendDebug(" with cmd id ");
//SendDebugln(next_command.id,DEC);
}
//}
}
// If the preload failed, return or just Loiter
// generate a dynamic command for RTL
// --------------------------------------------
if(next_command.id == CMD_BLANK){
if(next_command.id == NO_COMMAND){
// we are out of commands!
gcs.send_text(SEVERITY_LOW,"out of commands!");
//SendDebug("MSG <load_next_command> out of commands, g.waypoint_index: ");
//SendDebugln(g.waypoint_index,DEC);
handle_no_commands();
}
}
@ -88,11 +100,12 @@ void process_next_command()
Log_Write_Cmd(g.waypoint_index, &next_command);
process_now();
}
}
/*
These functions implement the waypoint commands.
*/
void process_must()
{
//SendDebug("process must index: ");
@ -109,12 +122,12 @@ void process_must()
// loads the waypoint into Next_WP struct
// --------------------------------------
set_next_WP(&next_command);
//set_next_WP(&next_command);
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = 0;
handle_process_must(command_must_ID);
handle_process_must();
next_command.id = NO_COMMAND;
}
void process_may()
@ -123,14 +136,14 @@ void process_may()
//Serial.println(g.waypoint_index,DEC);
command_may_ID = next_command.id;
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = 0;
gcs.send_text(SEVERITY_LOW,"<process_may> New may command loaded:");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
handle_process_may(command_may_ID);
handle_process_may();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = NO_COMMAND;
}
void process_now()
@ -139,14 +152,12 @@ void process_now()
//Serial.print("process_now cmd# ");
//Serial.println(g.waypoint_index,DEC);
byte id = next_command.id;
gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
handle_process_now();
// invalidate command so a new one is loaded
// -----------------------------------------
next_command.id = 0;
gcs.send_text(SEVERITY_LOW, "<process_now> New now command loaded: ");
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
handle_process_now(id);
next_command.id = NO_COMMAND;
}

6
ArduCopterMega/defines.h
View File

@ -87,7 +87,7 @@
#define ALT_HOLD 2 // AUTO control
#define FBW 3 // AUTO control
#define AUTO 4 // AUTO control
#define LOITER 5 // AUTO control
#define GCS_AUTO 5 // AUTO control
#define POSITION_HOLD 6 // Hold a single location
#define RTL 7 // AUTO control
#define TAKEOFF 8 // controlled decent rate
@ -95,7 +95,9 @@
#define NUM_MODES 10
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
#define CMD_BLANK 0x00 // there is no command stored in the mem location requested
#define CMD_BLANK 0 // there is no command stored in the mem location requested
#define NO_COMMAND 0
//repeating events

2
ArduCopterMega/motors.pde
View File

@ -36,7 +36,6 @@ void
set_servos_4()
{
static byte num;
static byte counteri;
int out_min;
// Quadcopter mix
@ -292,3 +291,4 @@ set_servos_4()
g.rc_4.control_in = ToDeg(dcm.yaw);
}
}

4
ArduCopterMega/navigation.pde
View File

@ -18,7 +18,7 @@ void navigate()
// waypoint distance from plane
// ----------------------------
wp_distance = getDistance(&current_loc, &next_WP);
wp_distance = get_distance(&current_loc, &next_WP);
if (wp_distance < 0){
gcs.send_text(SEVERITY_HIGH,"<navigate> WP error - distance < 0");
@ -184,7 +184,7 @@ long get_altitude_above_home(void)
return current_loc.alt - home.alt;
}
long getDistance(struct Location *loc1, struct Location *loc2)
long get_distance(struct Location *loc1, struct Location *loc2)
{
if(loc1->lat == 0 || loc1->lng == 0)
return -1;

8
ArduCopterMega/radio.pde
View File

@ -70,7 +70,13 @@ void read_radio()
//throttle_failsafe(g.rc_3.radio_in);
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), g.rc_1.control_in, g.rc_2.control_in, g.rc_3.control_in, g.rc_4.control_in);
/*
Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
g.rc_1.control_in,
g.rc_2.control_in,
g.rc_3.control_in,
g.rc_4.control_in);
*/
}
void throttle_failsafe(uint16_t pwm)

7
ArduCopterMega/read_me.text
View File

@ -15,11 +15,11 @@ CLI interactive setup - You must go through each item and set the values to matc
"setup" menu:
erase - run this first! erases bad values from EEPROMS just in case
reset - Performs factory reset and initialization of EEPROM values
reset - Performs factory reset and initialization of EEPROM values
radio - records the limits of ALL radio channels - very important!
pid - restores default PID values
frame - sets your frame config: [x, +, tri]
motors - interactive setup of your ESC and motors, enter this mode, then plug-in battery,
motors - interactive setup of your ESC and motors, enter this mode, then plug-in battery,
point at motors to make them spin
throttle will output full range to each motor - this is good for esc calibration
level - sets initial value of accelerometers - hold copter level
@ -55,7 +55,7 @@ Alt_hold - You need to set your g. value by toggling ch_7 for less than 1 seco
altitude is controlled by the throttle lever using absolute position - from 0 to 40 meters.
this control method will change after testing.
FBW - Simulates GPS Hold with the sticks being the position offset. Manual Throttle.
position_hold
position_hold
- 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.
RTL - Will try and fly back to home at the current altitude.
Auto - Will fly the mission loaded by the Waypoint writer
@ -64,7 +64,6 @@ Auto - Will fly the mission loaded by the Waypoint writer
what's new to commands for ACM:
- CMD_ANGLE - will set the desired yaw with control of angle/second and direction.
- CMD_ALTITUDE - will send the copter up from current position to desired altitude
- CMD_R_WAYPOINT - is just like a waypoint but relative to home
- CMD_TRACK_WAYPOINT - coming soon, will point the copter at the waypoint no matter the position
Special note:

8
ArduCopterMega/system.pde
View File

@ -261,11 +261,11 @@ void set_mode(byte mode)
break;
case STABILIZE:
set_current_loc_here();
do_hold_position();
break;
case ALT_HOLD:
set_current_loc_here();
do_hold_position();
break;
case AUTO:
@ -273,11 +273,11 @@ void set_mode(byte mode)
break;
case POSITION_HOLD:
set_current_loc_here();
do_hold_position();
break;
case RTL:
return_to_launch();
do_RTL();
break;
case TAKEOFF:

8
ArduCopterMega/test.pde
View File

@ -136,10 +136,10 @@ test_radio(uint8_t argc, const Menu::arg *argv)
g.rc_4.calc_pwm();
Serial.printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\n"),
(g.rc_1.control_in),
(g.rc_2.control_in),
(g.rc_3.control_in),
(g.rc_4.control_in),
g.rc_1.control_in,
g.rc_2.control_in,
g.rc_3.control_in,
g.rc_4.control_in,
g.rc_5.control_in,
g.rc_6.control_in,
g.rc_7.control_in);