Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 17e2475914c006b1b54860036bb0bf9f65aa4c0d
|
From: Jérémie D. <Ba...@us...> - 2010-02-16 17:33:11
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via 17e2475914c006b1b54860036bb0bf9f65aa4c0d (commit)
from 8adb2a66b9012493643ed81af92932b4494415ba (commit)
Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.
- Log -----------------------------------------------------------------
commit 17e2475914c006b1b54860036bb0bf9f65aa4c0d
Author: Jérémie Dimino <je...@di...>
Date: Tue Feb 16 18:08:28 2010 +0100
[driver] rewrite the driver with card monitoring
-----------------------------------------------------------------------
Changes:
diff --git a/PC_Mainboard/driver/card.ml b/PC_Mainboard/driver/card.ml
index 2c517d1..cd8c920 100644
--- a/PC_Mainboard/driver/card.ml
+++ b/PC_Mainboard/driver/card.ml
@@ -110,6 +110,7 @@ and state =
and wrapper = {
mutable state : state;
name : string;
+ watch : [ `Error of exn | `Closed ] Lwt.t;
}
type t = wrapper
@@ -119,6 +120,8 @@ let closed wrapper = match wrapper.state with
| Opened _ -> false
| Closed _ -> true
+let watch wrapper = wrapper.watch
+
(* Return a running card, if possible. *)
let rec get_card wrapper = match wrapper.state with
| Opened card ->
@@ -234,6 +237,15 @@ let rec make ~name ~handle =
} and wrapper = {
state = Opened card;
name = name;
+ watch = (try_lwt
+ lwt _ = abort_waiter in
+ (* Never happen: *)
+ return `Closed
+ with
+ | Card_closed ->
+ return `Closed
+ | exn ->
+ return (`Error exn))
} in
ignore (dispatch card);
return wrapper
diff --git a/PC_Mainboard/driver/card.mli b/PC_Mainboard/driver/card.mli
index c0ad590..d435a21 100644
--- a/PC_Mainboard/driver/card.mli
+++ b/PC_Mainboard/driver/card.mli
@@ -19,6 +19,11 @@ val name : t -> string
val closed : t -> bool
(** Returns [true] iff the card has been closed *)
+val watch : t -> [ `Error of exn | `Closed ] Lwt.t
+ (** [watch card] is a thread which is wakeup when the card is
+ closed, or when a fatal error happen. The argument describe the
+ reason. *)
+
exception Card_closed
(** Exception raised when trying to use a closed card *)
diff --git a/PC_Mainboard/driver/commands.ml b/PC_Mainboard/driver/commands.ml
index c298091..8524f16 100644
--- a/PC_Mainboard/driver/commands.ml
+++ b/PC_Mainboard/driver/commands.ml
@@ -28,6 +28,9 @@ let get_board_info card =
let bootloader card =
Card.send_command card Protocol.cmd_bootloader ""
+let reset card =
+ Card.send_command card Protocol.cmd_reset ""
+
module Compass =
struct
let get card =
diff --git a/PC_Mainboard/driver/commands.mli b/PC_Mainboard/driver/commands.mli
index 7c0862c..ac1fe16 100644
--- a/PC_Mainboard/driver/commands.mli
+++ b/PC_Mainboard/driver/commands.mli
@@ -14,6 +14,7 @@
val get_firmware_build : Card.t -> string Lwt.t
val get_board_info : Card.t -> string Lwt.t
val bootloader : Card.t -> unit Lwt.t
+val reset : Card.t -> unit Lwt.t
module Compass : sig
val get : Card.t -> int Lwt.t
diff --git a/PC_Mainboard/driver/driver.ml b/PC_Mainboard/driver/driver.ml
index 0c6adec..f103fb2 100644
--- a/PC_Mainboard/driver/driver.ml
+++ b/PC_Mainboard/driver/driver.ml
@@ -12,6 +12,8 @@
open OBus_pervasives
open Lwt
+let crashed card = fail (Failure(Printf.sprintf "%s card has crashed" (Card.name card)))
+
(* +-----------------------------------------------------------------+
| Compass |
+-----------------------------------------------------------------+ *)
@@ -46,13 +48,14 @@ struct
loop dev
let make card path =
+ lwt data = Commands.Compass.get card in
let dev = {
obus = OBus_object.make path;
card = card;
- data = 0;
+ data = data;
} in
ignore (loop dev);
- dev
+ return dev
end
(* +-----------------------------------------------------------------+
@@ -77,10 +80,11 @@ struct
fun dev id pos speed ->
Commands.AX12.goto dev.card id pos speed
- let make card path = {
- obus = OBus_object.make path;
- card = card;
- }
+ let make card path =
+ return {
+ obus = OBus_object.make path;
+ card = card;
+ }
end
(* +-----------------------------------------------------------------+
@@ -123,7 +127,7 @@ struct
data = Array.create 16 false;
} in
ignore (loop dev);
- dev
+ return dev
end
(* +-----------------------------------------------------------------+
@@ -166,12 +170,9 @@ struct
data = Array.create 8 0;
} in
ignore (loop dev);
- dev
+ return dev
end
-open OBus_pervasives
-open Lwt
-
(* +-----------------------------------------------------------------+
| Motors |
+-----------------------------------------------------------------+ *)
@@ -380,8 +381,7 @@ struct
speed_left = 0;
speed_right = 0;
} in
- dev.state <- React.S.map (fun state -> dev.acceleration <- 0) (Card.state card);
- dev
+ return dev
end
(* +-----------------------------------------------------------------+
@@ -389,6 +389,10 @@ end
+-----------------------------------------------------------------+ *)
let done_waiter, done_wakener = Lwt.wait ()
+let quit = ref false
+let card_interface = ref None
+let card_sensor = ref None
+let card_motor = ref None
module Manager =
struct
@@ -401,13 +405,19 @@ struct
include OBus.MakeInterface(struct let name = "fr.krobot.Manager" end)
- OL_method Shutdown : unit = fun () ->
+ let close card = match !card with
+ | None ->
+ return ()
+ | Some card ->
+ Card.close card
+
+ OL_method Shutdown : OBus_connection.t -> unit = fun () connection ->
Log#info "exiting";
- lwt bus = Lazy.force OBus_bus.system in
- lwt () = Card.close card_interface
- and () = Card.close card_sensor
- and () = Card.close card_motor
- and _ = OBus_bus.release_name bus "fr.krobot" in
+ quit := true;
+ lwt _ = OBus_bus.release_name connection "fr.krobot"
+ and () = close card_interface
+ and () = close card_sensor
+ and () = close card_motor in
Lwt.wakeup done_wakener ();
return ()
end
@@ -421,7 +431,6 @@ struct
type t = {
card : Card.t;
obus : OBus_object.t;
- mutable state : unit Lwt.t React.signal;
}
module OBus = OBus_object.Make(struct
@@ -434,69 +443,65 @@ struct
OL_property_r Name : string = fun card ->
return (Card.name card.card)
- let string_of_azt data =
- try
- String.sub data 0 (String.index data '\000')
- with Not_found ->
- data
-
OL_method GetFirmwareBuild : string = fun dev ->
Commands.get_firmware_build dev.card
OL_method GetBoardInfo : string = fun dev ->
Commands.get_board_info dev.card
- OL_method GetState : Types.card_state = fun dev ->
- return (React.S.value (Card.state dev.card))
-
OL_method Bootloader : unit = fun dev ->
Commands.bootloader dev.card
OL_method Reset : unit = fun dev ->
- Card.restart dev.card
-
- OL_signal StateChanged : Types.card_state
+ Commands.reset dev.card
let make card path =
- let dev = {
+ return {
card = card;
obus = OBus_object.make path;
- state = React.S.const (return ());
- } in
- dev.state <- React.S.map (state_changed dev) (Card.state dev.card);
- dev
+ }
end
(* +-----------------------------------------------------------------+
- | Cards |
+ | Cards management |
+-----------------------------------------------------------------+ *)
-
-
-let card_interface = Card.open_card ~name:"interface" ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_robot_interface
-let card_sensor = Card.open_card ~name:"sensor" ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_proximity_sensor
-let card_motor = Card.open_card ~name:"motor" ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_motor_controller
-
-(* Enable motors when a card comes up *)
-let on_running card f =
- let stop = ref ignore in
- let notifier =
- Lwt_signal.notify_p
- (function
- | `Running ->
- f card
- | `Opening ->
- return ()
- | `Closed ->
- !stop ();
- return ())
- (Card.state card)
- in
- stop := (fun () -> Lwt_signal.disable notifier)
-
-let () =
- on_running card_interface Commands.Motor.enable;
- on_running card_motor (fun card -> Commands.Motor.enable card <&> Commands.Motor.init_lm629 card)
+(* Continously try to open the card with given parameters *)
+let rec monitor_card ~name ~vendor_id ~product_id ~var on_up on_down =
+ try_bind
+ (fun () ->
+ return (USB.open_device_with ~vendor_id ~product_id))
+ (fun handle ->
+ Log#info "%s card opened" name;
+ lwt () =
+ try_lwt
+ lwt card = Card.make name handle in
+ var := Some card;
+ Log#info "%s card is up and running" name;
+ lwt () = on_up card in
+ lwt result = Card.watch card in
+ var := None;
+ lwt () = on_down () in
+ match result with
+ | `Closed ->
+ return ()
+ | `Error exn ->
+ Log#exn exn "%s card crashed with" name;
+ return ()
+ with exn ->
+ Log#exn exn "failed to initialise %s card" name;
+ return ()
+ in
+ if !quit then
+ return ()
+ else
+ monitor_card ~name ~vendor_id ~product_id ~var on_up on_down)
+ (fun exn ->
+ Log#exn exn "failed to open %s card" name;
+ if !quit then
+ return ()
+ else
+ monitor_card ~name ~vendor_id ~product_id ~var on_up on_down)
(* +-----------------------------------------------------------------+
| Entry point |
@@ -511,9 +516,6 @@ lwt () =
let usage_msg = Printf.sprintf "Usage: %s [-n]\n\noptions are:" (Filename.basename (Sys.argv.(0))) in
Arg.parse args ignore usage_msg;
- (* Be verbose: *)
- Lwt_log.set_level !Lwt_log.default (min Lwt_log.Info Lwt_log.default_level);
-
(* Open the krobot message bus *)
lwt bus = OBus_bus.of_addresses (OBus_address.of_string Config.bus_address) in
@@ -525,7 +527,7 @@ lwt () =
~interface:"fr.krobot.Manager"
~member:"Shutdown"
~destination:"fr.krobot"
- <:obus_func< unit >>
+ <:obus_func< unit >>
with OBus_bus.Service_unknown _ ->
return ()
in
@@ -544,38 +546,54 @@ lwt () =
if !foreground then
(* Running foreground, prints message on stderr: *)
- Log#info "starting krobot driver in foreground mode"
+ Log#notice "starting krobot driver in foreground mode"
else begin
- Log#info "starting krobot driver in daemon mode";
- (* In daemon mode, send messages to syslog: *)
- Lwt_log.default := Lwt_log.syslog
- ~level:(min Lwt_log.Info Lwt_log.default_level)
- ~facility:`Daemon
- ();
- (* Become a daemon. Keep stderr for possible error messages of
- libusb. *)
- Lwt_unix.daemonize ~keep_stderr:true ()
+ Log#notice "starting krobot driver in daemon mode";
+ Lwt_daemon.daemonize ()
end;
- Log#info "creating and exporting objects";
-
- (* Interface card *)
- Compass.OBus.export bus (Compass.make card_interface ["fr"; "krobot"; "Devices"; "Compass"]);
- AX12.OBus.export bus (AX12.make card_interface ["fr"; "krobot"; "Devices"; "AX12"]);
-
- (* Sensor card *)
- Logic_sensors.OBus.export bus (Logic_sensors.make card_sensor ["fr"; "krobot"; "Devices"; "LogicSensors"]);
- Range_finders.OBus.export bus (Range_finders.make card_sensor ["fr"; "krobot"; "Devices"; "RangeFinders"]);
+ ignore begin
+ monitor_card ~name:"interace" ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_robot_interface ~var:card_interface
+ (fun card ->
+ lwt () = Commands.Motor.enable card in
+ lwt () = MCard.OBus.export bus =|< MCard.make card ["fr"; "krobot"; "Cards"; "Interface"]
+ and () = Compass.OBus.export bus =|< Compass.make card ["fr"; "krobot"; "Devices"; "Compass"]
+ and () = AX12.OBus.export bus =|< AX12.make card ["fr"; "krobot"; "Devices"; "AX12"] in
+ return ())
+ (fun () ->
+ OBus_object.remove_by_path bus ["fr"; "krobot"; "Cards"; "Interface"];
+ OBus_object.remove_by_path bus ["fr"; "krobot"; "Devices"; "Compass"];
+ OBus_object.remove_by_path bus ["fr"; "krobot"; "Devices"; "AX12"];
+ return ())
+ end;
- (* Motor card *)
- Motors.OBus.export bus (Motors.make card_motor ["fr"; "krobot"; "Devices"; "Motors"]);
+ ignore begin
+ monitor_card ~name:"motor" ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_motor_controller ~var:card_motor
+ (fun card ->
+ lwt () = Commands.Motor.enable card and () = Commands.Motor.init_lm629 card in
+ lwt () = MCard.OBus.export bus =|< MCard.make card ["fr"; "krobot"; "Cards"; "Motor"]
+ and () = Motors.OBus.export bus =|< Motors.make card ["fr"; "krobot"; "Devices"; "Motors"] in
+ return ())
+ (fun () ->
+ OBus_object.remove_by_path bus ["fr"; "krobot"; "Cards"; "Motor"];
+ OBus_object.remove_by_path bus ["fr"; "krobot"; "Devices"; "Motors"];
+ return ())
+ end;
- (* Cards *)
- MCard.OBus.export bus (MCard.make card_interface ["fr"; "krobot"; "Cards"; "Interface"]);
- MCard.OBus.export bus (MCard.make card_sensor ["fr"; "krobot"; "Cards"; "Sensor"]);
- MCard.OBus.export bus (MCard.make card_motor ["fr"; "krobot"; "Cards"; "Motor"]);
+ ignore begin
+ monitor_card ~name:"sensor" ~vendor_id:Protocol.usb_vid ~product_id:Protocol.usb_pid_proximity_sensor ~var:card_sensor
+ (fun card ->
+ lwt () = MCard.OBus.export bus =|< MCard.make card ["fr"; "krobot"; "Cards"; "Sensor"]
+ and () = Logic_sensors.OBus.export bus =|< Logic_sensors.make card ["fr"; "krobot"; "Devices"; "LogicSensors"]
+ and () = Range_finders.OBus.export bus =|< Range_finders.make card ["fr"; "krobot"; "Devices"; "RangeFinders"] in
+ return ())
+ (fun () ->
+ OBus_object.remove_by_path bus ["fr"; "krobot"; "Cards"; "Sensor"];
+ OBus_object.remove_by_path bus ["fr"; "krobot"; "Devices"; "LogicSensors"];
+ OBus_object.remove_by_path bus ["fr"; "krobot"; "Devices"; "RangeFinders"];
+ return ())
+ end;
- (* Internal objects *)
Manager.OBus.export bus ();
Log#info "ready, waiting for requests";
hooks/post-receive
--
krobot
|
