Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

pitch handling added

Browse Source
This commit is contained in:
Pascal Brisset
2006-10-17 16:54:20 +00:00
parent 37ed8a6903
commit 11d0b98e3a
5 changed files with 49 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/sim/sim_ir.c
+2 -1
View File
@@ -14,8 +14,9 @@
void ir_gain_calib(void) {
}
value set_ir(value roll, value top) {
value set_ir(value roll, value front, value top) {
ir_roll = Int_val(roll);
ir_pitch = Int_val(front);
ir_top = Int_val(top);
return Val_unit;
}
sw/simulator/flightModel.ml
+40 -35
View File
@@ -42,22 +42,17 @@ type state = {
mutable z : meter;
mutable psi : radian; (* Trigonometric *)
mutable phi : radian;
mutable theta : radian;
mutable phi_dot : radian_s;
mutable delta_a : float;
mutable delta_b : float;
mutable thrust : float;
mutable air_speed : meter_s
}
let init route = {
start = Unix.gettimeofday (); t = 0.; x = 0.; y = 0. ; z = 0.;
psi = route; phi = 0.; phi_dot = 0.;
delta_a = 0.; thrust = 0.; air_speed = 0.
}
let get_xyz state = (state.x, state.y, state.z)
let get_time state = state.t
let get_phi state = state.phi
let get_attitude state = (state.phi, 0., state.psi)
let get_attitude state = (state.phi, state.theta, state.psi)
let set_air_speed state x = state.air_speed <- x
@@ -104,32 +99,6 @@ module Make(A:Data.MISSION) = struct
(* Minimum complexity *)
(*
http://controls.ae.gatech.edu/papers/johnson_dasc_01.pdf
http://controls.ae.gatech.edu/papers/johnson_mst_01.pdf
*)
let state_update = fun state (wx, wy) dt ->
let now = state.t +. dt in
if state.air_speed > 0. then begin
let phi_dot_dot = roll_response_factor *. state.delta_a +. c_lp *. state.phi_dot /. state.air_speed in
state.phi_dot <- state.phi_dot +. phi_dot_dot *. dt;
state.phi <- bound (state.phi +. state.phi_dot *. dt) (-.max_phi) max_phi;
let psi_dot = -. g /. state.air_speed *. tan (yaw_response_factor *. state.phi) in
state.psi <- norm_angle (state.psi +. psi_dot *. dt);
let dx = state.air_speed *. cos state.psi *. dt +. wx *. dt
and dy = state.air_speed *. sin state.psi *. dt +. wy *. dt in
state.x <- state.x +.dx ;
state.y <- state.y +. dy;
let gamma = (state.thrust -. drag) /. weight in
let dz = sin gamma *. state.air_speed *. dt in
state.z <- state.z +. dz
end;
state.t <- now
let commands =
try
let l = ExtXml.child A.ac.airframe "commands" in
@@ -153,12 +122,14 @@ module Make(A:Data.MISSION) = struct
let adc_roll_neutral = ios (defined_value infrared_section "ADC_ROLL_NEUTRAL")
let roll_neutral_default = rad_of_deg (float_value infrared_section "ROLL_NEUTRAL_DEFAULT")
let pitch_neutral_default = rad_of_deg (float_value infrared_section "PITCH_NEUTRAL_DEFAULT")
let min_thrust = 0
let max_thrust = max_pprz
let command_throttle = command "THROTTLE"
let command_roll = command "ROLL"
let command_pitch = command "PITCH"
let float_attrib = fun x a -> float_of_string (ExtXml.attrib x a)
let int_attrib = fun x a -> int_of_string (ExtXml.attrib x a)
@@ -167,7 +138,41 @@ module Make(A:Data.MISSION) = struct
let do_commands = fun state commands ->
let c_lda = 4e-5 in (* FIXME *)
state.delta_a <- -. c_lda *. float commands.(command_roll);
state.delta_b <- float commands.(command_pitch);
state.thrust <- (float (commands.(command_throttle) - min_thrust) /. float (max_thrust - min_thrust))
let nb_commands = 10 (* FIXME *)
end
let init route = {
start = Unix.gettimeofday (); t = 0.; x = 0.; y = 0. ; z = 0.;
psi = route; phi = 0.; phi_dot = 0.;
delta_a = 0.; delta_b = 0.; thrust = 0.; air_speed = 0.;
theta = pitch_neutral_default;
}
(* Minimum complexity *)
(*
http://controls.ae.gatech.edu/papers/johnson_dasc_01.pdf
http://controls.ae.gatech.edu/papers/johnson_mst_01.pdf
*)
let state_update = fun state (wx, wy) dt ->
let now = state.t +. dt in
if state.air_speed > 0. then begin
let phi_dot_dot = roll_response_factor *. state.delta_a +. c_lp *. state.phi_dot /. state.air_speed in
state.phi_dot <- state.phi_dot +. phi_dot_dot *. dt;
state.phi <- bound (state.phi +. state.phi_dot *. dt) (-.max_phi) max_phi;
let psi_dot = -. g /. state.air_speed *. tan (yaw_response_factor *. state.phi) in
state.psi <- norm_angle (state.psi +. psi_dot *. dt);
let dx = state.air_speed *. cos state.psi *. dt +. wx *. dt
and dy = state.air_speed *. sin state.psi *. dt +. wy *. dt in
state.x <- state.x +.dx ;
state.y <- state.y +. dy;
let gamma = (state.thrust -. drag) /. weight +. state.theta in
let dz = sin gamma *. state.air_speed *. dt in
state.z <- state.z +. dz;
(* Awfull: just to respond to the controller ... *)
state.theta <- state.theta +. 0.1 *. (4e-4 *. state.delta_b -. state.theta)
end;
state.t <- now
end (* Make functor *)
sw/simulator/flightModel.mli
+2 -3
View File
@@ -30,11 +30,8 @@ type radian = float
type radian_s = float
type state
val init : radian -> state
val get_xyz : state -> meter * meter * meter
val get_time : state -> float
val get_phi : state -> radian
val get_attitude : state -> radian * radian * radian
val set_air_speed : state -> meter_s -> unit
@@ -42,10 +39,12 @@ val set_air_speed : state -> meter_s -> unit
module Make :
functor (A : Data.MISSION) ->
sig
val init : radian -> state
val do_commands : state -> Stdlib.pprz_t array -> unit
val nb_commands : int
val nominal_airspeed : float (* m/s *)
val roll_neutral_default : float (* rad *)
val pitch_neutral_default : float (* rad *)
val state_update : state -> float * float -> float -> unit
(** [state_update state (wind_x, wind_y) dt] With m/s for wind and s for
dt *)
sw/simulator/sim.ml
+3 -3
View File
@@ -109,7 +109,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
let compute_gps_state = Gps.state () in
let initial_state = FlightModel.init (pi/.2. -. qfu/.180.*.pi) in
let initial_state = FM.init (pi/.2. -. qfu/.180.*.pi) in
let state = ref initial_state in
@@ -150,7 +150,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
FM.state_update !state (!wind_x, !wind_y) fm_period
and ir_task = fun () ->
let phi = FlightModel.get_phi !state in
let phi, theta, _ = FlightModel.get_attitude !state in
let horizon_distance = 1000. in
try
match !last_gps_state with
@@ -166,7 +166,7 @@ module Make(AircraftItl : AIRCRAFT_ITL) = struct
0. in
let phi = phi +. FM.roll_neutral_default in
let ir_left = (phi +. delta_ir ) *. !infrared_contrast
and ir_front = 0.
and ir_front = (FM.pitch_neutral_default +. theta) *. !infrared_contrast
and ir_top = pi /. 2. *. !infrared_contrast in
Aircraft.infrared ir_left ir_front ir_top
with
sw/simulator/sitl.ml
+2 -2
View File
@@ -197,10 +197,10 @@ module Make(A:Data.MISSION) = struct
(* Functions called by the simulator *)
let commands = fun s -> rcommands := s
external set_ir : int -> int -> unit = "set_ir"
external set_ir : int -> int -> int -> unit = "set_ir"
let infrared = fun ir_left ir_front ir_top ->
(** ADC neutral is not taken into account in the soft sim (c.f. sim_ir.c)*)
set_ir (truncate ir_left) (truncate ir_top)
set_ir (truncate ir_left) (truncate ir_front) (truncate ir_top)
external use_gps_pos: int -> int -> int -> float -> float -> float -> float -> float -> bool -> unit = "sim_use_gps_pos_bytecode" "sim_use_gps_pos"
let gps = fun gps ->