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

2.0.27

Browse Source 
Fixed GCS updates, 
Clear GCS Streamrates by default.
moved fast PWM functions to run all the time (for quads, experiment)
cleaned up Mavlink a bit.




git-svn-id: https://arducopter.googlecode.com/svn/trunk@2678 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-06-26 06:38:11 +00:00
parent ebfb132f34
commit 4f75842cbc
4 changed files with 465 additions and 510 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ArduCopterMega/ArduCopterMega.pde
View File

@ -528,10 +528,17 @@ void loop()
if (millis() - fiftyhz_loopTimer > 19) { if (millis() - fiftyhz_loopTimer > 19) {
delta_ms_fiftyhz = millis() - fiftyhz_loopTimer; delta_ms_fiftyhz = millis() - fiftyhz_loopTimer;
fiftyhz_loopTimer = millis(); fiftyhz_loopTimer = millis();
//PORTK |= B01000000;
// reads all of the necessary trig functions for cameras, throttle, etc.
update_trig();
medium_loop(); medium_loop();
// Stuff to run at full 50hz, but after the loops
fifty_hz_loop(); fifty_hz_loop();
counter_one_herz++; counter_one_herz++;
if(counter_one_herz == 50){ if(counter_one_herz == 50){
@ -550,10 +557,13 @@ void loop()
resetPerfData(); resetPerfData();
} }
} }
//PORTK &= B10111111;
} }
//PORTK &= B11101111; //PORTK &= B11101111;
} }
// PORTK |= B01000000;
// PORTK &= B10111111;
//
// Main loop // Main loop
void fast_loop() void fast_loop()
{ {
@ -589,10 +599,6 @@ void fast_loop()
void medium_loop() void medium_loop()
{ {
// reads all of the necessary trig functions for cameras, throttle, etc.
update_trig();
// This is the start of the medium (10 Hz) loop pieces // This is the start of the medium (10 Hz) loop pieces
// ----------------------------------------- // -----------------------------------------
switch(medium_loopCounter) { switch(medium_loopCounter) {
@ -690,9 +696,7 @@ void medium_loop()
// perform next command // perform next command
// -------------------- // --------------------
if(control_mode == AUTO){ if(control_mode == AUTO){
//if(home_is_set){
update_commands(); update_commands();
//}
} }
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
@ -712,9 +716,9 @@ void medium_loop()
gcs.send_message(MSG_ATTITUDE); // Sends attitude data gcs.send_message(MSG_ATTITUDE); // Sends attitude data
#endif #endif
//#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
// hil.data_stream_send(5,45); hil.data_stream_send(5,45);
//#endif #endif
if (g.log_bitmask & MASK_LOG_MOTORS) if (g.log_bitmask & MASK_LOG_MOTORS)
Log_Write_Motors(); Log_Write_Motors();
@ -826,6 +830,7 @@ void slow_loop()
loop_step = 6; loop_step = 6;
slow_loopCounter++; slow_loopCounter++;
superslow_loopCounter++; superslow_loopCounter++;
Serial.printf("sc: %4.4f \n", imu._sensor_compensation(0,50));
if(superslow_loopCounter > 800){ // every 4 minutes if(superslow_loopCounter > 800){ // every 4 minutes
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
@ -882,10 +887,9 @@ void slow_loop()
gcs.send_message(MSG_CPU_LOAD, load*100); gcs.send_message(MSG_CPU_LOAD, load*100);
#endif #endif
// XXX This was taking 14ms for no reason!!! #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
//#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0) hil.data_stream_send(1,5);
//hil.data_stream_send(1,5); #endif
//#endif
#if CHANNEL_6_TUNING != CH6_NONE #if CHANNEL_6_TUNING != CH6_NONE
tuning(); tuning();

164
ArduCopterMega/GCS_Mavlink.pde
View File

@ -14,14 +14,15 @@ waypoint_receive_timeout(1000), // 1 second
// stream rates // stream rates
_group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")), _group (key, key == Parameters::k_param_streamrates_port0 ? PSTR("SR0_"): PSTR("SR3_")),
streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")), // AP_VAR //ref //index, default, name
streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")), streamRateRawSensors (&_group, 0, 0, PSTR("RAW_SENS")),
streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")), streamRateExtendedStatus (&_group, 1, 0, PSTR("EXT_STAT")),
streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")), streamRateRCChannels (&_group, 2, 0, PSTR("RC_CHAN")),
streamRatePosition (&_group, 4, 0, PSTR("POSITION")), streamRateRawController (&_group, 3, 0, PSTR("RAW_CTRL")),
streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")), streamRatePosition (&_group, 4, 0, PSTR("POSITION")),
streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")), streamRateExtra1 (&_group, 5, 0, PSTR("EXTRA1")),
streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3")) streamRateExtra2 (&_group, 6, 0, PSTR("EXTRA2")),
streamRateExtra3 (&_group, 7, 0, PSTR("EXTRA3"))
{ {
} }
@ -38,6 +39,17 @@ 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
@ -82,6 +94,18 @@ 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)
{ {
@ -89,6 +113,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
send_message(MSG_RAW_IMU); send_message(MSG_RAW_IMU);
//Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get());
} }
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
@ -97,32 +122,40 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
send_message(MSG_NAV_CONTROLLER_OUTPUT); send_message(MSG_NAV_CONTROLLER_OUTPUT);
//Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get());
} }
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
send_message(MSG_LOCATION); // sent with GPS read
//send_message(MSG_LOCATION);
//Serial.printf("mav3 %d\n", (int)streamRatePosition.get());
} }
if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { if (freqLoopMatch(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
send_message(MSG_SERVO_OUT); send_message(MSG_SERVO_OUT);
//Serial.printf("mav4 %d\n", (int)streamRateRawController.get());
} }
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
send_message(MSG_RADIO_OUT); send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN); send_message(MSG_RADIO_IN);
//Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get());
} }
if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE); send_message(MSG_ATTITUDE);
//Serial.printf("mav6 %d\n", (int)streamRateExtra1.get());
} }
if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
send_message(MSG_VFR_HUD); send_message(MSG_VFR_HUD);
//Serial.printf("mav7 %d\n", (int)streamRateExtra2.get());
} }
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
// Available datastream // Available datastream
//Serial.printf("mav8 %d\n", (int)streamRateExtra3.get());
} }
} }
} }
@ -188,7 +221,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
streamRatePosition = freq; streamRatePosition = freq;
streamRateExtra1 = freq; streamRateExtra1 = freq;
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!!
break; break;
case MAV_DATA_STREAM_RAW_SENSORS: case MAV_DATA_STREAM_RAW_SENSORS:
@ -196,33 +231,35 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// we will not continue to broadcast raw sensor data at 50Hz. // we will not continue to broadcast raw sensor data at 50Hz.
break; break;
case MAV_DATA_STREAM_EXTENDED_STATUS: case MAV_DATA_STREAM_EXTENDED_STATUS:
streamRateExtendedStatus.set_and_save(freq); //streamRateExtendedStatus.set_and_save(freq);
streamRateExtendedStatus = freq;
break; break;
case MAV_DATA_STREAM_RC_CHANNELS: case MAV_DATA_STREAM_RC_CHANNELS:
streamRateRCChannels.set_and_save(freq); streamRateRCChannels = freq;
break; break;
case MAV_DATA_STREAM_RAW_CONTROLLER: case MAV_DATA_STREAM_RAW_CONTROLLER:
streamRateRawController.set_and_save(freq); streamRateRawController = freq;
break; break;
//case MAV_DATA_STREAM_RAW_SENSOR_FUSION: //case MAV_DATA_STREAM_RAW_SENSOR_FUSION:
// streamRateRawSensorFusion.set_and_save(freq); // streamRateRawSensorFusion.set_and_save(freq);
// break; // break;
case MAV_DATA_STREAM_POSITION: case MAV_DATA_STREAM_POSITION:
streamRatePosition.set_and_save(freq); streamRatePosition = freq;
break; break;
case MAV_DATA_STREAM_EXTRA1: case MAV_DATA_STREAM_EXTRA1:
streamRateExtra1.set_and_save(freq); streamRateExtra1 = freq;
break; break;
case MAV_DATA_STREAM_EXTRA2: case MAV_DATA_STREAM_EXTRA2:
streamRateExtra2.set_and_save(freq); streamRateExtra2 = freq;
//streamRateExtra2.set_and_save(freq);
break; break;
case MAV_DATA_STREAM_EXTRA3: case MAV_DATA_STREAM_EXTRA3:
streamRateExtra3.set_and_save(freq); streamRateExtra3 = freq;
break; break;
default: default:
@ -755,13 +792,13 @@ 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); ((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); ((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); ((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;
@ -804,94 +841,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
} }
#endif #endif
#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 switch
} // end handle mavlink } // end handle mavlink

9
ArduCopterMega/motors_quad.pde
View File

@ -64,9 +64,6 @@ void output_motors_armed()
APM_RC.OutputCh(CH_2, motor_out[CH_2]); APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]); APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]); APM_RC.OutputCh(CH_4, motor_out[CH_4]);
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
}else{ }else{
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
@ -78,10 +75,10 @@ void output_motors_armed()
APM_RC.OutputCh(CH_2, motor_out[CH_2]); APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]); APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]); APM_RC.OutputCh(CH_4, motor_out[CH_4]);
#endif
// InstantPWM // InstantPWM
APM_RC.Force_Out0_Out1(); APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3(); APM_RC.Force_Out2_Out3();
#endif
} }
void output_motors_disarmed() void output_motors_disarmed()
@ -102,6 +99,10 @@ void output_motors_disarmed()
APM_RC.OutputCh(CH_2, 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_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
} }
void output_motor_test() void output_motor_test()

4
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.26 Beta", main_menu_commands); MENU(main_menu, "AC 2.0.27 Beta", main_menu_commands);
void init_ardupilot() void init_ardupilot()
{ {
@ -101,7 +101,7 @@ void init_ardupilot()
// XXX set Analog out 14 to output // XXX set Analog out 14 to output
// 76543210 // 76543210
// DDRK |= B01010000; //DDRK |= B01010000;
#if MOTOR_LEDS == 1 #if MOTOR_LEDS == 1
pinMode(FR_LED, OUTPUT); // GPS status LED pinMode(FR_LED, OUTPUT); // GPS status LED