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

APO velocity/ altitude working.

Browse Source
This commit is contained in:
Wenyao Xie 2011-12-06 19:38:23 -05:00
parent 2432ae130d
commit c4203631dc
4 changed files with 12 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
apo/QuadArducopter.h
View File

@ -55,7 +55,7 @@ static const bool rangeFinderUpEnabled = false;
static const bool rangeFinderDownEnabled = false;
// loop rates
static const float loopRate = 250; // attitude nav
static const float loopRate = 200; // attitude nav
static const float loop0Rate = 50; // controller
static const float loop1Rate = 10; // pos nav/ gcs fast
static const float loop2Rate = 1; // gcs slow

4
libraries/APO/AP_Autopilot.cpp
View File

@ -221,10 +221,10 @@ void AP_Autopilot::callback2(void * data) {
*/
if (apo->getHal()->gcs) {
// send messages
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW_INT);
//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW_INT);
//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION);
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW);
//apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION);
apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
}

10
libraries/APO/AP_CommLink.cpp
View File

@ -117,9 +117,9 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
case MAVLINK_MSG_ID_GPS_RAW: {
mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(),
_hal->gps->latitude/1.0e7,
_hal->gps->longitude/1.0e7, _hal->gps->altitude/10.0, 0, 0,
_hal->gps->ground_speed*10.0,
_hal->gps->ground_course*10.0);
_hal->gps->longitude/1.0e7, _hal->gps->altitude/100.0, 0, 0,
_hal->gps->ground_speed/100.0,
_hal->gps->ground_course/10.0);
break;
}
@ -127,8 +127,8 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(),
_hal->gps->latitude,
_hal->gps->longitude, _hal->gps->altitude*10.0, 0, 0,
_hal->gps->ground_speed*10.0,
_hal->gps->ground_course*10.0);
_hal->gps->ground_speed/100.0,
_hal->gps->ground_course/10.0);
break;
}

5
libraries/APO/DcmNavigator.cpp
View File

@ -135,7 +135,7 @@ void DcmNavigator::updateFast(float dt) {
_hal->baro->Read(); // Get new data from absolute pressure sensor
float reference = 44330 * (1.0 - (pow(_groundPressure.get()/101325.0,0.190295)));
setAlt(_baroLowPass.update((44330 * (1.0 - (pow((_hal->baro->Press/101325.0),0.190295)))) - reference,dt));
_hal->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_hal->baro->Press,_hal->baro->Temp);
//_hal->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_hal->baro->Press,_hal->baro->Temp);
// last resort, use gps altitude
} else if (_hal->gps && _hal->gps->fix) {
@ -151,6 +151,9 @@ void DcmNavigator::updateFast(float dt) {
setRollRate(_dcm.get_gyro().x);
setPitchRate(_dcm.get_gyro().y);
setYawRate(_dcm.get_gyro().z);
setXAccel(_dcm.get_accel().x);
setYAccel(_dcm.get_accel().y);
setZAccel(_dcm.get_accel().z);
/*
* accel/gyro debug