5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-02 22:18:28 -04:00

Copter: update current_loc at 400hz

This commit is contained in:
Randy Mackay 2016-04-19 16:16:06 +09:00
parent f45c69e7ad
commit 13c26eab67
4 changed files with 4 additions and 20 deletions

View File

@ -300,9 +300,6 @@ void Copter::rc_loop()
// ---------------------------
void Copter::throttle_loop()
{
// get altitude and climb rate from inertial lib
read_inertial_altitude();
// update throttle_low_comp value (controls priority of throttle vs attitude control)
update_throttle_thr_mix();

View File

@ -877,7 +877,6 @@ private:
bool heli_stabilize_init(bool ignore_checks);
void heli_stabilize_run();
void read_inertia();
void read_inertial_altitude();
bool land_complete_maybe();
void update_land_and_crash_detectors();
void update_land_detector();
@ -906,7 +905,6 @@ private:
void motors_output();
void lost_vehicle_check();
void run_nav_updates(void);
void calc_position();
void calc_distance_and_bearing();
void calc_wp_distance();
void calc_wp_bearing();

View File

@ -7,11 +7,11 @@ void Copter::read_inertia()
{
// inertial altitude estimates
inertial_nav.update(G_Dt);
}
// read_inertial_altitude - pull altitude and climb rate from inertial nav library
void Copter::read_inertial_altitude()
{
// pull position from interial nav library
current_loc.lng = inertial_nav.get_longitude();
current_loc.lat = inertial_nav.get_latitude();
// exit immediately if we do not have an altitude estimate
if (!inertial_nav.get_filter_status().flags.vert_pos) {
return;

View File

@ -7,9 +7,6 @@
// To-Do - rename and move this function to make it's purpose more clear
void Copter::run_nav_updates(void)
{
// fetch position from inertial navigation
calc_position();
// calculate distance and bearing for reporting and autopilot decisions
calc_distance_and_bearing();
@ -17,14 +14,6 @@ void Copter::run_nav_updates(void)
run_autopilot();
}
// calc_position - get lat and lon positions from inertial nav library
void Copter::calc_position()
{
// pull position from interial nav library
current_loc.lng = inertial_nav.get_longitude();
current_loc.lat = inertial_nav.get_latitude();
}
// calc_distance_and_bearing - calculate distance and bearing to next waypoint and home
void Copter::calc_distance_and_bearing()
{