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

fix for Logging, motors_out buffer overrun, formatting.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1769 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-03-11 21:37:37 +00:00
parent 20fdbda315
commit 732446eb3a
8 changed files with 160 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopterMega/ArduCopterMega.pde
View File

@ -205,7 +205,7 @@ const char* flight_mode_strings[] = {
// Radio // Radio
// ----- // -----
int motor_out[4]; int motor_out[8];
Vector3f omega; Vector3f omega;
// Failsafe // Failsafe

239
ArduCopterMega/GCS_Mavlink.pde
View File

@ -52,19 +52,13 @@ GCS_MAVLINK::update(void)
_queued_send(); _queued_send();
// stop waypoint sending if timeout // stop waypoint sending if timeout
if (global_data.waypoint_sending && if (global_data.waypoint_sending && (millis() - global_data.waypoint_timelast_send) > global_data.waypoint_send_timeout){
millis() - global_data.waypoint_timelast_send >
global_data.waypoint_send_timeout)
{
send_text(SEVERITY_LOW,"waypoint send timeout"); send_text(SEVERITY_LOW,"waypoint send timeout");
global_data.waypoint_sending = false; global_data.waypoint_sending = false;
} }
// stop waypoint receiving if timeout // stop waypoint receiving if timeout
if (global_data.waypoint_receiving && if (global_data.waypoint_receiving && (millis() - global_data.waypoint_timelast_receive) > global_data.waypoint_receive_timeout){
millis() - global_data.waypoint_timelast_receive >
global_data.waypoint_receive_timeout)
{
send_text(SEVERITY_LOW,"waypoint receive timeout"); send_text(SEVERITY_LOW,"waypoint receive timeout");
global_data.waypoint_receiving = false; global_data.waypoint_receiving = false;
} }
@ -74,7 +68,7 @@ void
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
{ {
if (freqLoopMatch(global_data.streamRateRawSensors,freqMin,freqMax)) if (freqLoopMatch(global_data.streamRateRawSensors,freqMin,freqMax))
send_message(MSG_RAW_IMU); send_message(MSG_RAW_IMU);
if (freqLoopMatch(global_data.streamRateExtendedStatus,freqMin,freqMax)) { if (freqLoopMatch(global_data.streamRateExtendedStatus,freqMin,freqMax)) {
send_message(MSG_EXTENDED_STATUS); send_message(MSG_EXTENDED_STATUS);
@ -84,7 +78,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
} }
if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax)) if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax))
send_message(MSG_LOCATION); send_message(MSG_LOCATION);
if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax)) { if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax)) {
// This is used for HIL. Do not change without discussing with HIL maintainers // This is used for HIL. Do not change without discussing with HIL maintainers
@ -100,10 +94,9 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
send_message(MSG_ATTITUDE); send_message(MSG_ATTITUDE);
if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax)) // Use Extra 3 for additional HIL info if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax)) // Use Extra 3 for additional HIL info
send_message(MSG_VFR_HUD); send_message(MSG_VFR_HUD);
if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax)) if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax)){
{
// Available datastream // Available datastream
} }
} }
@ -146,14 +139,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
switch(packet.req_stream_id) switch(packet.req_stream_id)
{ {
case MAV_DATA_STREAM_ALL: case MAV_DATA_STREAM_ALL:
global_data.streamRateRawSensors = freq; global_data.streamRateRawSensors = freq;
global_data.streamRateExtendedStatus = freq; global_data.streamRateExtendedStatus = freq;
global_data.streamRateRCChannels = freq; global_data.streamRateRCChannels = freq;
global_data.streamRateRawController = freq; global_data.streamRateRawController = freq;
global_data.streamRatePosition = freq; global_data.streamRatePosition = freq;
global_data.streamRateExtra1 = freq; global_data.streamRateExtra1 = freq;
global_data.streamRateExtra2 = freq; global_data.streamRateExtra2 = freq;
global_data.streamRateExtra3 = freq; global_data.streamRateExtra3 = freq;
break; break;
case MAV_DATA_STREAM_RAW_SENSORS: case MAV_DATA_STREAM_RAW_SENSORS:
global_data.streamRateRawSensors = freq; global_data.streamRateRawSensors = freq;
@ -197,89 +190,88 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// do action // do action
send_text(SEVERITY_LOW,"action received"); send_text(SEVERITY_LOW,"action received");
switch(packet.action) switch(packet.action){
{
case MAV_ACTION_LAUNCH: case MAV_ACTION_LAUNCH:
//set_mode(TAKEOFF); //set_mode(TAKEOFF);
break; break;
case MAV_ACTION_RETURN: case MAV_ACTION_RETURN:
set_mode(RTL); set_mode(RTL);
break; break;
case MAV_ACTION_EMCY_LAND: case MAV_ACTION_EMCY_LAND:
//set_mode(LAND); //set_mode(LAND);
break; break;
case MAV_ACTION_HALT: case MAV_ACTION_HALT:
do_hold_position(); do_hold_position();
break; break;
// can't implement due to APM configuration // can't implement due to APM configuration
// just setting to manual to be safe // just setting to manual to be safe
case MAV_ACTION_MOTORS_START: case MAV_ACTION_MOTORS_START:
case MAV_ACTION_CONFIRM_KILL: case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_EMCY_KILL: case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_MOTORS_STOP: case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN: case MAV_ACTION_SHUTDOWN:
set_mode(ACRO); set_mode(ACRO);
break; break;
case MAV_ACTION_CONTINUE: case MAV_ACTION_CONTINUE:
process_next_command(); process_next_command();
break; break;
case MAV_ACTION_SET_MANUAL: case MAV_ACTION_SET_MANUAL:
set_mode(ACRO); set_mode(ACRO);
break; break;
case MAV_ACTION_SET_AUTO: case MAV_ACTION_SET_AUTO:
set_mode(AUTO); set_mode(AUTO);
break; break;
case MAV_ACTION_STORAGE_READ: case MAV_ACTION_STORAGE_READ:
AP_Var::load_all(); AP_Var::load_all();
break; break;
case MAV_ACTION_STORAGE_WRITE: case MAV_ACTION_STORAGE_WRITE:
AP_Var::save_all(); AP_Var::save_all();
break; break;
case MAV_ACTION_CALIBRATE_RC: break; case MAV_ACTION_CALIBRATE_RC: break;
trim_radio(); trim_radio();
break; break;
case MAV_ACTION_CALIBRATE_GYRO: case MAV_ACTION_CALIBRATE_GYRO:
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_ground(); startup_ground();
break; break;
case MAV_ACTION_REC_START: break; case MAV_ACTION_REC_START: break;
case MAV_ACTION_REC_PAUSE: break; case MAV_ACTION_REC_PAUSE: break;
case MAV_ACTION_REC_STOP: break; case MAV_ACTION_REC_STOP: break;
case MAV_ACTION_TAKEOFF: case MAV_ACTION_TAKEOFF:
//set_mode(TAKEOFF); //set_mode(TAKEOFF);
break; break;
case MAV_ACTION_NAVIGATE: case MAV_ACTION_NAVIGATE:
set_mode(AUTO); set_mode(AUTO);
break; break;
case MAV_ACTION_LAND: case MAV_ACTION_LAND:
//set_mode(LAND); //set_mode(LAND);
break; break;
case MAV_ACTION_LOITER: case MAV_ACTION_LOITER:
//set_mode(LOITER); //set_mode(LOITER);
break; break;
default: break; default: break;
} }
break; break;
} }
@ -290,17 +282,22 @@ 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)) break;
if (mavlink_check_target(packet.target_system,packet.target_component))
break;
// Start sending waypoints // Start sending waypoints
mavlink_msg_waypoint_count_send(chan,msg->sysid, mavlink_msg_waypoint_count_send(
msg->compid,g.waypoint_total + 1); // + home chan,msg->sysid,
global_data.waypoint_timelast_send = millis(); msg->compid,
global_data.waypoint_sending = true; g.waypoint_total + 1); // + home
global_data.waypoint_receiving = false;
global_data.waypoint_dest_sysid = msg->sysid; global_data.waypoint_timelast_send = millis();
global_data.waypoint_dest_compid = msg->compid; global_data.waypoint_sending = true;
global_data.requested_interface = chan; global_data.waypoint_receiving = false;
global_data.waypoint_dest_sysid = msg->sysid;
global_data.waypoint_dest_compid = msg->compid;
global_data.requested_interface = chan;
break; break;
} }
@ -444,9 +441,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} }
g.waypoint_total.set_and_save(packet.count - 1); g.waypoint_total.set_and_save(packet.count - 1);
global_data.waypoint_timelast_receive = millis(); global_data.waypoint_timelast_receive = millis();
global_data.waypoint_receiving = true; global_data.waypoint_receiving = true;
global_data.waypoint_sending = false; global_data.waypoint_sending = false;
global_data.waypoint_request_i = 0; global_data.waypoint_request_i = 0;
break; break;
} }
@ -520,7 +517,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
if (global_data.waypoint_request_i > g.waypoint_total) if (global_data.waypoint_request_i > g.waypoint_total)
{ {
uint8_t type = 0; // ok (0), error(1) uint8_t type = 0; // ok (0), error(1)
mavlink_msg_waypoint_ack_send(chan,msg->sysid,msg->compid,type);
mavlink_msg_waypoint_ack_send(
chan,
msg->sysid,
msg->compid,
type);
send_text(SEVERITY_LOW,"flight plan received"); send_text(SEVERITY_LOW,"flight plan received");
global_data.waypoint_receiving = false; global_data.waypoint_receiving = false;
// XXX ignores waypoint radius for individual waypoints, can // XXX ignores waypoint radius for individual waypoints, can
@ -574,9 +577,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} }
// Report back new value // Report back new value
mavlink_msg_param_value_send(chan, (int8_t *)key, packet.param_value, mavlink_msg_param_value_send(
_count_parameters(), chan,
-1); // XXX we don't actually know what its index is... (int8_t *)key,
packet.param_value,
_count_parameters(),
-1); // XXX we don't actually know what its index is...
break; break;
} // end case } // end case
@ -748,14 +754,10 @@ GCS_MAVLINK::_queued_send()
AP_Var *vp; AP_Var *vp;
float value; float value;
// copy the current parameter and prepare to move to the next // copy the current parameter and prepare to move to the next
vp = _queued_parameter; vp = _queued_parameter;
_queued_parameter = _queued_parameter->next(); _queued_parameter = _queued_parameter->next();
// if the parameter can be cast to float, report it here and break out of the loop // if the parameter can be cast to float, report it here and break out of the loop
value = vp->cast_to_float(); value = vp->cast_to_float();
if (!isnan(value)) { if (!isnan(value)) {
@ -763,11 +765,12 @@ GCS_MAVLINK::_queued_send()
char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK
vp->copy_name(param_name, sizeof(param_name)); vp->copy_name(param_name, sizeof(param_name));
mavlink_msg_param_value_send(chan, mavlink_msg_param_value_send(
(int8_t*)param_name, chan,
value, (int8_t*)param_name,
_count_parameters(), value,
_queued_parameter_index); _count_parameters(),
_queued_parameter_index);
_queued_parameter_index++; _queued_parameter_index++;
break; break;
@ -783,12 +786,16 @@ GCS_MAVLINK::_queued_send()
// request waypoints one by one // request waypoints one by one
// XXX note that this is pan-interface // XXX note that this is pan-interface
if (global_data.waypoint_receiving && global_data.requested_interface == chan && if (global_data.waypoint_receiving &&
global_data.waypoint_request_i <= g.waypoint_total && mavdelay > 15) // limits to 3.33 hz (global_data.requested_interface == chan) &&
global_data.waypoint_request_i <= (g.waypoint_total && mavdelay > 15)) // limits to 3.33 hz
{ {
mavlink_msg_waypoint_request_send(chan, mavlink_msg_waypoint_request_send(
global_data.waypoint_dest_sysid, chan,
global_data.waypoint_dest_compid ,global_data.waypoint_request_i); global_data.waypoint_dest_sysid,
global_data.waypoint_dest_compid,
global_data.waypoint_request_i);
mavdelay = 0; mavdelay = 0;
} }
} }

4
ArduCopterMega/Parameters.h
View File

@ -239,12 +239,12 @@ public:
throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")),
throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
flight_mode_channel (FLIGHT_MODE_CHANNEL+1, k_param_flight_mode_channel, PSTR("FLT_MODE_CH")), flight_mode_channel (FLIGHT_MODE_CHANNEL+1, k_param_flight_mode_channel, PSTR("FLT_MODE_CH")),
flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")), flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")),
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")), pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
log_bitmask (0, k_param_log_bitmask, PSTR("LOG_BITMASK")), log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")),
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),

1
ArduCopterMega/defines.h
View File

@ -204,6 +204,7 @@
#define MASK_LOG_RAW (1<<7) #define MASK_LOG_RAW (1<<7)
#define MASK_LOG_CMD (1<<8) #define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CUR (1<<9) #define MASK_LOG_CUR (1<<9)
#define MASK_LOG_SET_DEFAULTS (1<<15)
// Waypoint Modes // Waypoint Modes
// ---------------- // ----------------

21
ArduCopterMega/motors.pde
View File

@ -279,20 +279,21 @@ set_servos_4()
motor_auto_safe = true; motor_auto_safe = true;
} }
// Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
// fill the motor_out[] array for HIL use // fill the motor_out[] array for HIL use
for (unsigned char i=0; i<8; i++) { for (unsigned char i = 0; i < 8; i++) {
motor_out[i] = 0; motor_out[i] = g.rc_3.radio_min;
} }
// Send commands to motors
APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
if (g.frame_type == HEXA_FRAME) { if (g.frame_type == HEXA_FRAME) {
APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(CH_8, motor_out[CH_8]);
} }
// reset I terms of PID controls // reset I terms of PID controls

1
ArduCopterMega/setup.pde
View File

@ -99,6 +99,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
AP_Var::erase_all(); AP_Var::erase_all();
Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue")); Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
delay(1000);
default_log_bitmask(); default_log_bitmask();
default_gains(); default_gains();

12
ArduCopterMega/system.pde
View File

@ -141,7 +141,8 @@ void init_ardupilot()
#endif #endif
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
if(g.compass_enabled) if(g.compass_enabled)
init_compass(); init_compass();
@ -199,6 +200,10 @@ void init_ardupilot()
if (g.log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Startup(TYPE_GROUNDSTART_MSG); Log_Write_Startup(TYPE_GROUNDSTART_MSG);
if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) {
default_log_bitmask();
}
// set the correct flight mode // set the correct flight mode
// --------------------------- // ---------------------------
reset_control_switch(); reset_control_switch();
@ -212,7 +217,7 @@ void startup_ground(void)
gcs.send_text(SEVERITY_LOW,"<startup_ground> GROUND START"); gcs.send_text(SEVERITY_LOW,"<startup_ground> GROUND START");
#if(GROUND_START_DELAY > 0) #if(GROUND_START_DELAY > 0)
gcs.send_text(SEVERITY_LOW,"<startup_ground> With Delay"); //gcs.send_text(SEVERITY_LOW," With Delay");
delay(GROUND_START_DELAY * 1000); delay(GROUND_START_DELAY * 1000);
#endif #endif
@ -302,9 +307,6 @@ void set_mode(byte mode)
// output control mode to the ground station // output control mode to the ground station
gcs.send_message(MSG_MODE_CHANGE); gcs.send_message(MSG_MODE_CHANGE);
if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
} }
void set_failsafe(boolean mode) void set_failsafe(boolean mode)

27
ArduCopterMega/test.pde
View File

@ -20,7 +20,7 @@ static int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
static int8_t test_mag(uint8_t argc, const Menu::arg *argv); static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); static int8_t test_eedump(uint8_t argc, const Menu::arg *argv);
static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv);
// This is the help function // This is the help function
// PSTR is an AVR macro to read strings from flash memory // PSTR is an AVR macro to read strings from flash memory
@ -64,7 +64,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"compass", test_mag}, {"compass", test_mag},
{"xbee", test_xbee}, {"xbee", test_xbee},
{"eedump", test_eedump}, {"eedump", test_eedump},
{"rawgps", test_rawgps}, {"rawgps", test_rawgps},
}; };
// A Macro to create the Menu // A Macro to create the Menu
@ -768,20 +768,20 @@ test_wp_print(struct Location *cmd, byte index)
static int8_t static int8_t
test_rawgps(uint8_t argc, const Menu::arg *argv) test_rawgps(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
delay(1000); delay(1000);
while(1){ while(1){
if (Serial3.available()) if (Serial3.available())
Serial1.write(Serial3.read()); Serial1.write(Serial3.read());
if (Serial1.available()) if (Serial1.available())
Serial3.write(Serial1.read()); Serial3.write(Serial1.read());
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);
} }
} }
} }
static int8_t static int8_t
@ -835,8 +835,9 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
update_alt(); update_alt();
output_auto_throttle(); output_auto_throttle();
Serial.printf_P(PSTR("Alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"), Serial.printf_P(PSTR("B_alt: %ld, S_alt: %ld, \tnext_alt: %ld \terror: %ld, \tcruise: %d, \tint: %6.2f \tout:%d\n"),
baro_alt, baro_alt,
sonar_alt,
next_WP.alt, next_WP.alt,
altitude_error, altitude_error,
(int)g.throttle_cruise, (int)g.throttle_cruise,